Support for saving and loading dynamic nav meshes @ppiastucki

[Upstream] from recast4j
506b503 - chore: Support for saving and loading dynamic nav meshes (fixes #200) (#209)

src/DotRecast.Detour.Dynamic/DtDynamicNavMesh.cs

@@ -23,6 +23,7 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Threading.Tasks;
 using DotRecast.Core;
+using DotRecast.Core.Collections;
 using DotRecast.Detour.Dynamic.Colliders;
 using DotRecast.Detour.Dynamic.Io;
 using DotRecast.Recast;
@@ -63,7 +64,7 @@ namespace DotRecast.Detour.Dynamic
             navMeshParams.orig.Y = voxelFile.bounds[1];
             navMeshParams.orig.Z = voxelFile.bounds[2];
             navMeshParams.tileWidth = voxelFile.useTiles ? voxelFile.cellSize * voxelFile.tileSizeX : voxelFile.bounds[3] - voxelFile.bounds[0];
-            navMeshParams.tileHeight = voxelFile.useTiles ? voxelFile.cellSize * voxelFile.tileSizeZ: voxelFile.bounds[5] - voxelFile.bounds[2];
+            navMeshParams.tileHeight = voxelFile.useTiles ? voxelFile.cellSize * voxelFile.tileSizeZ : voxelFile.bounds[5] - voxelFile.bounds[2];
             navMeshParams.maxTiles = voxelFile.tiles.Count;
             navMeshParams.maxPolys = 0x8000;
             foreach (var t in voxelFile.tiles)
@@ -230,6 +231,8 @@ namespace DotRecast.Detour.Dynamic
         {
             if (_dirty)
             {
+                _dirty = false;
+
                 DtNavMesh navMesh = new DtNavMesh();
                 navMesh.Init(navMeshParams, MAX_VERTS_PER_POLY);
 
@@ -239,7 +242,6 @@ namespace DotRecast.Detour.Dynamic
                 }
 
                 _navMesh = navMesh;
-                _dirty = false;
                 return true;
             }
 
@@ -267,5 +269,21 @@ namespace DotRecast.Detour.Dynamic
         {
             return _tiles.Values.Select(t => t.recastResult).ToList();
         }
+
+        public void NavMesh(DtNavMesh mesh)
+        {
+            _tiles.Values.ForEach(t =>
+            {
+                const int MAX_NEIS = 32;
+                DtMeshTile[] tiles = new DtMeshTile[MAX_NEIS];
+                int nneis = mesh.GetTilesAt(t.voxelTile.tileX, t.voxelTile.tileZ, tiles, MAX_NEIS);
+                if (0 < nneis)
+                {
+                    t.SetMeshData(tiles[0].data);
+                }
+            });
+            _navMesh = mesh;
+            _dirty = false;
+        }
     }
 }

src/DotRecast.Detour.Dynamic/DtDynamicTile.cs

@@ -189,5 +189,10 @@ namespace DotRecast.Detour.Dynamic
                 id = 0;
             }
         }
+
+        public void SetMeshData(DtMeshData data)
+        {
+            this.meshData = data;
+        }
     }
 }

test/DotRecast.Detour.Dynamic.Test/DynamicNavMeshTest.cs

@@ -1,3 +1,4 @@
+using System;
 using System.Collections.Generic;
 using System.IO;
 using System.Threading.Tasks;
@@ -6,6 +7,7 @@ using DotRecast.Core.Numerics;
 using DotRecast.Detour.Dynamic.Colliders;
 using DotRecast.Detour.Dynamic.Io;
 using DotRecast.Detour.Dynamic.Test.Io;
+using DotRecast.Detour.Io;
 using NUnit.Framework;
 
 namespace DotRecast.Detour.Dynamic.Test;
@@ -78,4 +80,101 @@ public class DynamicNavMeshTest
         // path length should be back to the initial value
         Assert.That(path.Count, Is.EqualTo(16));
     }
+
+
+    [Test]
+    public void ShouldSaveAndLoadDynamicNavMesh()
+    {
+        using var writerMs = new MemoryStream();
+        using var bw = new BinaryWriter(writerMs);
+
+
+        int maxVertsPerPoly = 6;
+        // load voxels from file
+
+        {
+            byte[] bytes = RcIO.ReadFileIfFound("test_tiles.voxels");
+            using var readMs = new MemoryStream(bytes);
+            using var br = new BinaryReader(readMs);
+
+            DtVoxelFileReader reader = new DtVoxelFileReader(DtVoxelTileLZ4ForTestCompressor.Shared);
+            DtVoxelFile f = reader.Read(br);
+
+            // create dynamic navmesh
+            DtDynamicNavMesh mesh = new DtDynamicNavMesh(f);
+
+            // build navmesh asynchronously using multiple threads
+            mesh.Build(Task.Factory);
+
+            // Save the resulting nav mesh and re-use it
+            new DtMeshSetWriter().Write(bw, mesh.NavMesh(), RcByteOrder.LITTLE_ENDIAN, true);
+            maxVertsPerPoly = mesh.NavMesh().GetMaxVertsPerPoly();
+        }
+
+        {
+            byte[] bytes = RcIO.ReadFileIfFound("test_tiles.voxels");
+            using var readMs = new MemoryStream(bytes);
+            using var br = new BinaryReader(readMs);
+
+            // load voxels from file
+            DtVoxelFileReader reader = new DtVoxelFileReader(DtVoxelTileLZ4ForTestCompressor.Shared);
+            DtVoxelFile f = reader.Read(br);
+
+            // create dynamic navmesh
+            DtDynamicNavMesh mesh = new DtDynamicNavMesh(f);
+            // use the saved nav mesh instead of building from scratch
+            DtNavMesh navMesh = new DtMeshSetReader().Read(new RcByteBuffer(writerMs.ToArray()), maxVertsPerPoly);
+            mesh.NavMesh(navMesh);
+
+            DtNavMeshQuery query = new DtNavMeshQuery(mesh.NavMesh());
+            IDtQueryFilter filter = new DtQueryDefaultFilter();
+
+            // find path
+            _ = query.FindNearestPoly(START_POS, EXTENT, filter, out var startNearestRef, out var startNearestPos, out var _);
+            _ = query.FindNearestPoly(END_POS, EXTENT, filter, out var endNearestRef, out var endNearestPos, out var _);
+
+            List<long> path = new List<long>();
+            query.FindPath(startNearestRef, endNearestRef, startNearestPos, endNearestPos, filter, ref path, DtFindPathOption.AnyAngle);
+
+            // check path length without any obstacles
+            Assert.That(path.Count, Is.EqualTo(16));
+
+            // place obstacle
+            DtCollider colldier = new DtSphereCollider(SPHERE_POS, 20, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GROUND, 0.1f);
+            long colliderId = mesh.AddCollider(colldier);
+
+            // update navmesh asynchronously
+            mesh.Update(Task.Factory);
+
+            // create new query
+            query = new DtNavMeshQuery(mesh.NavMesh());
+
+            // find path again
+            _ = query.FindNearestPoly(START_POS, EXTENT, filter, out startNearestRef, out startNearestPos, out var _);
+            _ = query.FindNearestPoly(END_POS, EXTENT, filter, out endNearestRef, out endNearestPos, out var _);
+
+            path = new List<long>();
+            query.FindPath(startNearestRef, endNearestRef, startNearestPos, endNearestPos, filter, ref path, DtFindPathOption.AnyAngle);
+
+            // check path length with obstacles
+            Assert.That(path.Count, Is.EqualTo(19));
+
+            // remove obstacle
+            mesh.RemoveCollider(colliderId);
+            // update navmesh asynchronously
+            mesh.Update(Task.Factory);
+
+            // create new query
+            query = new DtNavMeshQuery(mesh.NavMesh());
+            // find path one more time
+            _ = query.FindNearestPoly(START_POS, EXTENT, filter, out startNearestRef, out startNearestPos, out var _);
+            _ = query.FindNearestPoly(END_POS, EXTENT, filter, out endNearestRef, out endNearestPos, out var _);
+
+            path = new List<long>();
+            query.FindPath(startNearestRef, endNearestRef, startNearestPos, endNearestPos, filter, ref path, DtFindPathOption.AnyAngle);
+
+            // path length should be back to the initial value
+            Assert.That(path.Count, Is.EqualTo(16));
+        }
+    }
 }