Optimize NavMesh builder when updating a large number of tiles

Source/Engine/Navigation/NavMeshBuilder.cpp

@@ -68,6 +68,11 @@ struct Modifier
     NavAreaProperties* NavArea;
 };
 
+struct TileId
+{
+    int32 X, Y, Layer;
+};
+
 struct NavSceneRasterizer
 {
     NavMesh* NavMesh;
@@ -198,13 +203,15 @@ struct NavSceneRasterizer
 
         // Transform vertices into world space vertex buffer
         vb.Resize(vertexCount);
+        Float3* vbData = vb.Get();
         for (int32 i = 0; i < vertexCount; i++)
-            vb[i] = sphere.Center + vertices[i] * sphere.Radius;
+            vbData[i] = sphere.Center + vertices[i] * sphere.Radius;
 
         // Generate index buffer
         const int32 stride = horizontalSegments + 1;
         int32 indexCount = 0;
         ib.Resize(verticalSegments * (horizontalSegments + 1) * 6);
+        int32* ibData = ib.Get();
         for (int32 i = 0; i < verticalSegments; i++)
         {
             const int32 nextI = i + 1;
@@ -212,13 +219,13 @@ struct NavSceneRasterizer
             {
                 const int32 nextJ = (j + 1) % stride;
 
-                ib[indexCount++] = i * stride + j;
+                ibData[indexCount++] = i * stride + j;
-                ib[indexCount++] = nextI * stride + j;
+                ibData[indexCount++] = nextI * stride + j;
-                ib[indexCount++] = i * stride + nextJ;
+                ibData[indexCount++] = i * stride + nextJ;
 
-                ib[indexCount++] = i * stride + nextJ;
+                ibData[indexCount++] = i * stride + nextJ;
-                ib[indexCount++] = nextI * stride + j;
+                ibData[indexCount++] = nextI * stride + j;
-                ib[indexCount++] = nextI * stride + nextJ;
+                ibData[indexCount++] = nextI * stride + nextJ;
             }
         }
     }
@@ -335,54 +342,8 @@ struct NavSceneRasterizer
     }
 };
 
-// Builds navmesh tile bounds and check if there are any valid navmesh volumes at that tile location
+void CancelNavMeshTileBuildTasks(NavMeshRuntime* runtime);
-// Returns true if tile is intersecting with any navmesh bounds volume actor - which means tile is in use
+void CancelNavMeshTileBuildTasks(NavMeshRuntime* runtime, int32 x, int32 y);
-bool GetNavMeshTileBounds(Scene* scene, NavMesh* navMesh, int32 x, int32 y, float tileSize, BoundingBox& tileBoundsNavMesh, const Matrix& worldToNavMesh)
-{
-    // Build initial tile bounds (with infinite extent)
-    tileBoundsNavMesh.Minimum.X = (float)x * tileSize;
-    tileBoundsNavMesh.Minimum.Y = -NAV_MESH_TILE_MAX_EXTENT;
-    tileBoundsNavMesh.Minimum.Z = (float)y * tileSize;
-    tileBoundsNavMesh.Maximum.X = tileBoundsNavMesh.Minimum.X + tileSize;
-    tileBoundsNavMesh.Maximum.Y = NAV_MESH_TILE_MAX_EXTENT;
-    tileBoundsNavMesh.Maximum.Z = tileBoundsNavMesh.Minimum.Z + tileSize;
-
-    // Check if any navmesh volume intersects with the tile
-    bool foundAnyVolume = false;
-    Vector2 rangeY;
-    for (int32 i = 0; i < scene->Navigation.Volumes.Count(); i++)
-    {
-        const auto volume = scene->Navigation.Volumes[i];
-        if (!volume->AgentsMask.IsNavMeshSupported(navMesh->Properties))
-            continue;
-        const auto& volumeBounds = volume->GetBox();
-        BoundingBox volumeBoundsNavMesh;
-        BoundingBox::Transform(volumeBounds, worldToNavMesh, volumeBoundsNavMesh);
-        if (volumeBoundsNavMesh.Intersects(tileBoundsNavMesh))
-        {
-            if (foundAnyVolume)
-            {
-                rangeY.X = Math::Min(rangeY.X, volumeBoundsNavMesh.Minimum.Y);
-                rangeY.Y = Math::Max(rangeY.Y, volumeBoundsNavMesh.Maximum.Y);
-            }
-            else
-            {
-                rangeY.X = volumeBoundsNavMesh.Minimum.Y;
-                rangeY.Y = volumeBoundsNavMesh.Maximum.Y;
-            }
-            foundAnyVolume = true;
-        }
-    }
-
-    if (foundAnyVolume)
-    {
-        // Build proper tile bounds
-        tileBoundsNavMesh.Minimum.Y = rangeY.X;
-        tileBoundsNavMesh.Maximum.Y = rangeY.Y;
-    }
-
-    return foundAnyVolume;
-}
 
 void RemoveTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, int32 layer)
 {
@@ -404,9 +365,10 @@ void RemoveTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, int
     runtime->RemoveTile(x, y, layer);
 }
 
-bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, BoundingBox& tileBoundsNavMesh, const Matrix& worldToNavMesh, float tileSize, rcConfig& config)
+bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, BoundingBox& tileBoundsNavMesh, const Matrix& worldToNavMesh, float tileSize, rcConfig& config, Task* task)
 {
     rcContext context;
+    context.enableLog(false);
     int32 layer = 0;
 
     // Expand tile bounds by a certain margin
@@ -464,6 +426,9 @@ bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, B
         }
     }
 
+    if (task->IsCancelRequested())
+        return false;
+
     {
         PROFILE_CPU_NAMED("FilterHeightfield");
         rcFilterLowHangingWalkableObstacles(&context, config.walkableClimb, *heightfield);
@@ -507,6 +472,9 @@ bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, B
         }
     }
 
+    if (task->IsCancelRequested())
+        return false;
+
     {
         PROFILE_CPU_NAMED("BuildDistanceField");
         if (!rcBuildDistanceField(&context, *compactHeightfield))
@@ -648,6 +616,9 @@ bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, B
         params.offMeshConUserID = offMeshId.Get();
     }
 
+    if (task->IsCancelRequested())
+        return false;
+
     // Generate navmesh tile data
     unsigned char* navData = nullptr;
     int navDataSize = 0;
@@ -661,6 +632,7 @@ bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, B
     }
     ASSERT_LOW_LAYER(navDataSize > 4 && *(uint32*)navData == DT_NAVMESH_MAGIC); // Sanity check for Detour header
 
+    if (!task->IsCancelRequested())
     {
         PROFILE_CPU_NAMED("CreateTiles");
         ScopeLock lock(runtime->Locker);
@@ -762,7 +734,7 @@ public:
         const auto navMesh = NavMesh.Get();
         if (!navMesh)
             return false;
-        if (GenerateTile(NavMesh, Runtime, X, Y, TileBoundsNavMesh, WorldToNavMesh, TileSize, Config))
+        if (GenerateTile(NavMesh, Runtime, X, Y, TileBoundsNavMesh, WorldToNavMesh, TileSize, Config, this))
         {
             LOG(Warning, "Failed to generate navmesh tile at {0}x{1}.", X, Y);
         }
@@ -779,6 +751,50 @@ public:
     }
 };
 
+void CancelNavMeshTileBuildTasks(NavMeshRuntime* runtime)
+{
+    NavBuildTasksLocker.Lock();
+    for (int32 i = 0; i < NavBuildTasks.Count(); i++)
+    {
+        auto task = NavBuildTasks[i];
+        if (task->Runtime == runtime)
+        {
+            NavBuildTasksLocker.Unlock();
+
+            // Cancel task but without locking queue from this thread to prevent deadlocks
+            task->Cancel();
+
+            NavBuildTasksLocker.Lock();
+            i--;
+            if (NavBuildTasks.IsEmpty())
+                break;
+        }
+    }
+    NavBuildTasksLocker.Unlock();
+}
+
+void CancelNavMeshTileBuildTasks(NavMeshRuntime* runtime, int32 x, int32 y)
+{
+    NavBuildTasksLocker.Lock();
+    for (int32 i = 0; i < NavBuildTasks.Count(); i++)
+    {
+        auto task = NavBuildTasks[i];
+        if (task->Runtime == runtime && task->X == x && task->Y == y)
+        {
+            NavBuildTasksLocker.Unlock();
+
+            // Cancel task but without locking queue from this thread to prevent deadlocks
+            task->Cancel();
+
+            NavBuildTasksLocker.Lock();
+            i--;
+            if (NavBuildTasks.IsEmpty())
+                break;
+        }
+    }
+    NavBuildTasksLocker.Unlock();
+}
+
 void OnSceneUnloading(Scene* scene, const Guid& sceneId)
 {
     // Cancel pending build requests
@@ -843,14 +859,15 @@ float NavMeshBuilder::GetNavMeshBuildingProgress()
 
 void BuildTileAsync(NavMesh* navMesh, const int32 x, const int32 y, const rcConfig& config, const BoundingBox& tileBoundsNavMesh, const Matrix& worldToNavMesh, float tileSize)
 {
+    PROFILE_CPU();
     NavMeshRuntime* runtime = navMesh->GetRuntime();
     NavBuildTasksLocker.Lock();
 
     // Skip if this tile is already during cooking
     for (int32 i = 0; i < NavBuildTasks.Count(); i++)
     {
-        const auto task = NavBuildTasks[i];
+        const auto task = NavBuildTasks.Get()[i];
-        if (task->X == x && task->Y == y && task->Runtime == runtime)
+        if (task->GetState() == TaskState::Queued && task->X == x && task->Y == y && task->Runtime == runtime)
         {
             NavBuildTasksLocker.Unlock();
             return;
@@ -899,11 +916,16 @@ void BuildDirtyBounds(Scene* scene, NavMesh* navMesh, const BoundingBox& dirtyBo
 
     {
         PROFILE_CPU_NAMED("Prepare");
+        runtime->Locker.Lock();
 
         // Prepare scene data and navmesh
         rebuild |= Math::NotNearEqual(navMesh->Data.TileSize, tileSize);
         if (rebuild)
         {
+            runtime->Locker.Unlock();
+            CancelNavMeshTileBuildTasks(runtime);
+            runtime->Locker.Lock();
+
             // Remove all tiles from navmesh runtime
             runtime->RemoveTiles(navMesh);
             runtime->SetTileSize(tileSize);
@@ -918,9 +940,10 @@ void BuildDirtyBounds(Scene* scene, NavMesh* navMesh, const BoundingBox& dirtyBo
         else
         {
             // Ensure to have enough memory for tiles
-            runtime->SetTileSize(tileSize);
             runtime->EnsureCapacity(tilesX * tilesY);
         }
+
+        runtime->Locker.Unlock();
     }
 
     // Initialize nav mesh configuration
@@ -931,21 +954,93 @@ void BuildDirtyBounds(Scene* scene, NavMesh* navMesh, const BoundingBox& dirtyBo
     {
         PROFILE_CPU_NAMED("StartBuildingTiles");
 
+        // Cache navmesh volumes
+        Array<BoundingBox, InlinedAllocation<8>> volumes;
+        for (int32 i = 0; i < scene->Navigation.Volumes.Count(); i++)
+        {
+            const auto volume = scene->Navigation.Volumes.Get()[i];
+            if (!volume->AgentsMask.IsNavMeshSupported(navMesh->Properties) ||
+                !volume->GetBox().Intersects(dirtyBoundsAligned))
+                continue;
+            auto& bounds = volumes.AddOne();
+            BoundingBox::Transform(volume->GetBox(), worldToNavMesh, bounds);
+        }
+
+        Array<TileId> unusedTiles;
+        Array<Pair<TileId, BoundingBox>> usedTiles;
         for (int32 y = tilesMin.Z; y < tilesMax.Z; y++)
         {
             for (int32 x = tilesMin.X; x < tilesMax.X; x++)
             {
+                // Build initial tile bounds (with infinite extent)
                 BoundingBox tileBoundsNavMesh;
-                if (GetNavMeshTileBounds(scene, navMesh, x, y, tileSize, tileBoundsNavMesh, worldToNavMesh))
+                tileBoundsNavMesh.Minimum.X = (float)x * tileSize;
+                tileBoundsNavMesh.Minimum.Y = -NAV_MESH_TILE_MAX_EXTENT;
+                tileBoundsNavMesh.Minimum.Z = (float)y * tileSize;
+                tileBoundsNavMesh.Maximum.X = tileBoundsNavMesh.Minimum.X + tileSize;
+                tileBoundsNavMesh.Maximum.Y = NAV_MESH_TILE_MAX_EXTENT;
+                tileBoundsNavMesh.Maximum.Z = tileBoundsNavMesh.Minimum.Z + tileSize;
+
+                // Check if any navmesh volume intersects with the tile
+                bool foundAnyVolume = false;
+                Vector2 rangeY;
+                for (const auto& bounds : volumes)
                 {
-                    BuildTileAsync(navMesh, x, y, config, tileBoundsNavMesh, worldToNavMesh, tileSize);
+                    if (bounds.Intersects(tileBoundsNavMesh))
+                    {
+                        if (foundAnyVolume)
+                        {
+                            rangeY.X = Math::Min(rangeY.X, bounds.Minimum.Y);
+                            rangeY.Y = Math::Max(rangeY.Y, bounds.Maximum.Y);
+                        }
+                        else
+                        {
+                            rangeY.X = bounds.Minimum.Y;
+                            rangeY.Y = bounds.Maximum.Y;
+                            foundAnyVolume = true;
+                        }
+                    }
+                }
+
+                // Check if tile is intersecting with any navmesh bounds volume actor - which means tile is in use
+                if (foundAnyVolume)
+                {
+                    // Setup proper tile bounds
+                    tileBoundsNavMesh.Minimum.Y = rangeY.X;
+                    tileBoundsNavMesh.Maximum.Y = rangeY.Y;
+                    usedTiles.Add({ { x, y, 0 }, tileBoundsNavMesh });
                 }
                 else
                 {
-                    RemoveTile(navMesh, runtime, x, y, 0);
+                    unusedTiles.Add({ x, y, 0 });
                 }
             }
         }
+
+        // Remove unused tiles
+        {
+            PROFILE_CPU_NAMED("RemoveUnused");
+            for (const auto& tile : unusedTiles)
+            {
+                // Wait for any async tasks that are producing this tile
+                CancelNavMeshTileBuildTasks(runtime, tile.X, tile.Y);
+            }
+            runtime->Locker.Lock();
+            for (const auto& tile : unusedTiles)
+            {
+                RemoveTile(navMesh, runtime, tile.X, tile.Y, 0);
+            }
+            runtime->Locker.Unlock();
+        }
+
+        // Build used tiles
+        {
+            PROFILE_CPU_NAMED("AddNew");
+            for (const auto& e : usedTiles)
+            {
+                BuildTileAsync(navMesh, e.First.X, e.First.Y, config, e.Second, worldToNavMesh, tileSize);
+            }
+        }
     }
 }
 
@@ -1025,7 +1120,7 @@ void BuildDirtyBounds(Scene* scene, const BoundingBox& dirtyBounds, bool rebuild
             NavBuildTasksLocker.Lock();
             for (int32 i = 0; i < NavBuildTasks.Count(); i++)
             {
-                if (NavBuildTasks[i]->NavMesh == navMesh)
+                if (NavBuildTasks.Get()[i]->NavMesh == navMesh)
                     usageCount++;
             }
             NavBuildTasksLocker.Unlock();
@@ -1064,7 +1159,7 @@ void NavMeshBuilder::Update()
     const auto now = DateTime::NowUTC();
     for (int32 i = 0; NavBuildQueue.HasItems() && i < NavBuildQueue.Count(); i++)
     {
-        auto req = NavBuildQueue[i];
+        auto req = NavBuildQueue.Get()[i];
         if (now - req.Time >= 0)
         {
             NavBuildQueue.RemoveAt(i--);
@@ -1118,7 +1213,7 @@ void NavMeshBuilder::Build(Scene* scene, float timeoutMs)
 
     for (int32 i = 0; i < NavBuildQueue.Count(); i++)
     {
-        auto& e = NavBuildQueue[i];
+        auto& e = NavBuildQueue.Get()[i];
         if (e.Scene == scene && e.DirtyBounds == req.DirtyBounds)
         {
             e = req;

Source/Engine/Navigation/NavMeshRuntime.cpp

@@ -15,6 +15,19 @@
 #define USE_DATA_LINK 0
 #define USE_NAV_MESH_ALLOC 0
 
+#if USE_NAV_MESH_ALLOC
+#define GET_NAV_TILE_DATA(tile) \
+    const int32 dataSize = (tile).Data.Length(); \
+    const auto flags = DT_TILE_FREE_DATA; \
+    const auto data = (byte*)dtAlloc(dataSize, DT_ALLOC_PERM); \
+    Platform::MemoryCopy(data, (tile).Data.Get(), dataSize)
+#else
+#define GET_NAV_TILE_DATA(tile) \
+    const int32 dataSize = (tile).Data.Length(); \
+    const auto flags = 0; \
+    const auto data = (tile).Data.Get()
+#endif
+
 namespace
 {
     FORCE_INLINE void InitFilter(dtQueryFilter& filter)
@@ -353,15 +366,7 @@ void NavMeshRuntime::EnsureCapacity(int32 tilesToAddCount)
     // Restore previous tiles
     for (auto& tile : _tiles)
     {
-        const int32 dataSize = tile.Data.Length();
+        GET_NAV_TILE_DATA(tile);
-#if USE_NAV_MESH_ALLOC
-        const auto flags = DT_TILE_FREE_DATA;
-        const auto data = (byte*)dtAlloc(dataSize, DT_ALLOC_PERM);
-        Platform::MemoryCopy(data, tile.Data.Get(), dataSize);
-#else
-		const auto flags = 0;
-		const auto data = tile.Data.Get();
-#endif
         const auto result = _navMesh->addTile(data, dataSize, flags, 0, nullptr);
         if (dtStatusFailed(result))
         {
@@ -661,15 +666,7 @@ void NavMeshRuntime::AddTileInternal(NavMesh* navMesh, NavMeshTileData& tileData
 #endif
 
     // Add tile to navmesh
-    const int32 dataSize = tile->Data.Length();
+    GET_NAV_TILE_DATA(*tile);
-#if USE_NAV_MESH_ALLOC
-    const auto flags = DT_TILE_FREE_DATA;
-    const auto data = (byte*)dtAlloc(dataSize, DT_ALLOC_PERM);
-    Platform::MemoryCopy(data, tile->Data.Get(), dataSize);
-#else
-	const auto flags = 0;
-	const auto data = tile->Data.Get();
-#endif
     const auto result = _navMesh->addTile(data, dataSize, flags, 0, nullptr);
     if (dtStatusFailed(result))
     {