refactor: dynamic update tool

src/DotRecast.Core/RcMatrix4x4f.cs

@@ -1,4 +1,6 @@
-namespace DotRecast.Core
+using System;
+
+namespace DotRecast.Core
 {
     public struct RcMatrix4x4f
     {
@@ -102,5 +104,44 @@
 
             return dest;
         }
+
+        public static RcMatrix4x4f Rotate(float a, float x, float y, float z)
+        {
+            var matrix = new RcMatrix4x4f();
+            a = (float)(a * Math.PI / 180.0); // convert to radians
+            float s = (float)Math.Sin(a);
+            float c = (float)Math.Cos(a);
+            float t = 1.0f - c;
+
+            float tx = t * x;
+            float ty = t * y;
+            float tz = t * z;
+
+            float sz = s * z;
+            float sy = s * y;
+            float sx = s * x;
+
+            matrix.M11 = tx * x + c;
+            matrix.M12 = tx * y + sz;
+            matrix.M13 = tx * z - sy;
+            matrix.M14 = 0;
+
+            matrix.M21 = tx * y - sz;
+            matrix.M22 = ty * y + c;
+            matrix.M23 = ty * z + sx;
+            matrix.M24 = 0;
+
+            matrix.M31 = tx * z + sy;
+            matrix.M32 = ty * z - sx;
+            matrix.M33 = tz * z + c;
+            matrix.M34 = 0;
+
+            matrix.M41 = 0;
+            matrix.M42 = 0;
+            matrix.M43 = 0;
+            matrix.M44 = 1;
+
+            return matrix;
+        }
     }
 }

src/DotRecast.Recast.Demo/Tools/DynamicUpdateSampleTool.cs

@@ -77,9 +77,7 @@ public class DynamicUpdateSampleTool : ISampleTool
 
     private DynamicColliderShape colliderShape = DynamicColliderShape.SPHERE;
 
-    private DynamicNavMesh dynaMesh;
     private readonly TaskFactory executor;
-    private readonly Dictionary<long, RcGizmo> colliderGizmos = new();
 
     private bool sposSet;
     private bool eposSet;
@@ -139,6 +137,8 @@ public class DynamicUpdateSampleTool : ISampleTool
 
             var saveVoxelPopupStrId = "Save Voxels Popup";
             bool isSaveVoxelPopup = true;
+
+            var dynaMesh = _tool.GetDynamicNavMesh();
             if (dynaMesh != null)
             {
                 ImGui.Checkbox("Compression", ref compression);
@@ -280,10 +280,10 @@ public class DynamicUpdateSampleTool : ISampleTool
         {
             if (showColliders)
             {
-                colliderGizmos.Values.ForEach(g =>
+                foreach (var gizmo in _tool.GetGizmos())
                 {
-                    GizmoRenderer.Render(renderer.GetDebugDraw(), g.Gizmo);
+                    GizmoRenderer.Render(renderer.GetDebugDraw(), gizmo.Gizmo);
-                });
+                }
             }
         }
 
@@ -360,48 +360,7 @@ public class DynamicUpdateSampleTool : ISampleTool
         {
             if (!shift)
             {
-                RcGizmo colliderWithGizmo = null;
+                _tool.AddShape(colliderShape, p);
-                if (dynaMesh != null)
-                {
-                    if (colliderShape == DynamicColliderShape.SPHERE)
-                    {
-                        colliderWithGizmo = _tool.SphereCollider(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.CAPSULE)
-                    {
-                        colliderWithGizmo = _tool.CapsuleCollider(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.BOX)
-                    {
-                        colliderWithGizmo = _tool.BoxCollider(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.CYLINDER)
-                    {
-                        colliderWithGizmo = _tool.CylinderCollider(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.COMPOSITE)
-                    {
-                        colliderWithGizmo = _tool.CompositeCollider(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.TRIMESH_BRIDGE)
-                    {
-                        colliderWithGizmo = _tool.TrimeshBridge(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.TRIMESH_HOUSE)
-                    {
-                        colliderWithGizmo = _tool.TrimeshHouse(p, dynaMesh.config.walkableClimb);
-                    }
-                    else if (colliderShape == DynamicColliderShape.CONVEX)
-                    {
-                        colliderWithGizmo = _tool.ConvexTrimesh(p, dynaMesh.config.walkableClimb);
-                    }
-                }
-
-                if (colliderWithGizmo != null)
-                {
-                    long id = dynaMesh.AddCollider(colliderWithGizmo.Collider);
-                    colliderGizmos.Add(id, colliderWithGizmo);
-                }
             }
         }
 
@@ -418,18 +377,14 @@ public class DynamicUpdateSampleTool : ISampleTool
                 epos = p;
             }
 
+            var dynaMesh = _tool.GetDynamicNavMesh();
             if (sposSet && eposSet && dynaMesh != null)
             {
-                RcVec3f sp = RcVec3f.Of(spos.x, spos.y + 1.3f, spos.z);
-                RcVec3f ep = RcVec3f.Of(epos.x, epos.y + 1.3f, epos.z);
                 long t1 = RcFrequency.Ticks;
-                bool hasHit = dynaMesh.VoxelQuery().Raycast(sp, ep, out var hitPos);
+                bool hasHit = _tool.Raycast(spos, epos, out var hitPos, out raycastHitPos);
                 long t2 = RcFrequency.Ticks;
                 raycastTime = (t2 - t1) / TimeSpan.TicksPerMillisecond;
                 raycastHit = hasHit;
-                raycastHitPos = hasHit
-                    ? RcVec3f.Of(sp.x + hitPos * (ep.x - sp.x), sp.y + hitPos * (ep.y - sp.y), sp.z + hitPos * (ep.z - sp.z))
-                    : ep;
             }
         }
     }
@@ -441,65 +396,21 @@ public class DynamicUpdateSampleTool : ISampleTool
         {
             if (shift)
             {
-                foreach (var e in colliderGizmos)
+                _tool.RemoveShape(start, dir);
-                {
-                    if (Hit(start, dir, e.Value.Collider.Bounds()))
-                    {
-                        dynaMesh.RemoveCollider(e.Key);
-                        colliderGizmos.Remove(e.Key);
-                        break;
-                    }
-                }
             }
         }
     }
 
-    private bool Hit(RcVec3f point, RcVec3f dir, float[] bounds)
-    {
-        float cx = 0.5f * (bounds[0] + bounds[3]);
-        float cy = 0.5f * (bounds[1] + bounds[4]);
-        float cz = 0.5f * (bounds[2] + bounds[5]);
-        float dx = 0.5f * (bounds[3] - bounds[0]);
-        float dy = 0.5f * (bounds[4] - bounds[1]);
-        float dz = 0.5f * (bounds[5] - bounds[2]);
-        float rSqr = dx * dx + dy * dy + dz * dz;
-        float mx = point.x - cx;
-        float my = point.y - cy;
-        float mz = point.z - cz;
-        float c = mx * mx + my * my + mz * mz - rSqr;
-        if (c <= 0.0f)
-        {
-            return true;
-        }
-
-        float b = mx * dir.x + my * dir.y + mz * dir.z;
-        if (b > 0.0f)
-        {
-            return false;
-        }
-
-        float disc = b * b - c;
-        return disc >= 0.0f;
-    }
-
-
     public void HandleUpdate(float dt)
-    {
-        if (dynaMesh != null)
-        {
-            UpdateDynaMesh();
-        }
-    }
-
-    private void UpdateDynaMesh()
     {
         long t = RcFrequency.Ticks;
         try
         {
-            bool updated = dynaMesh.Update(executor).Result;
+            bool updated = _tool.UpdateDynaMesh(executor);
             if (updated)
             {
                 buildTime = (RcFrequency.Ticks - t) / TimeSpan.TicksPerMillisecond;
+                var dynaMesh = _tool.GetDynamicNavMesh();
                 _sample.Update(null, dynaMesh.RecastResults(), dynaMesh.NavMesh());
                 _sample.SetChanged(false);
             }
@@ -515,31 +426,26 @@ public class DynamicUpdateSampleTool : ISampleTool
     {
         try
         {
-            var voxelFile = _tool.Load(filename, DtVoxelTileLZ4DemoCompressor.Shared);
+            var dynaMesh = _tool.Load(filename, DtVoxelTileLZ4DemoCompressor.Shared);
-            dynaMesh = new DynamicNavMesh(voxelFile);
-            dynaMesh.config.keepIntermediateResults = true;
 
             UpdateFrom(dynaMesh.config);
             BuildDynaMesh();
-
-            colliderGizmos.Clear();
         }
         catch (Exception e)
         {
             Logger.Error(e, "");
-            dynaMesh = null;
         }
     }
 
 
     private void Save(string filename)
     {
-        VoxelFile voxelFile = VoxelFile.From(dynaMesh);
+        _tool.Save(filename, compression, DtVoxelTileLZ4DemoCompressor.Shared);
-        _tool.Save(filename, voxelFile, compression, DtVoxelTileLZ4DemoCompressor.Shared);
     }
 
     private void BuildDynaMesh()
     {
+        var dynaMesh = _tool.GetDynamicNavMesh();
         UpdateTo(dynaMesh.config);
         long t = RcFrequency.Ticks;
         try

src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs

@@ -1,7 +1,9 @@
 using System;
+using System.Collections.Generic;
 using System.IO;
-
+using System.Threading.Tasks;
 using DotRecast.Core;
+using DotRecast.Detour.Dynamic;
 using DotRecast.Detour.Dynamic.Colliders;
 using DotRecast.Detour.Dynamic.Io;
 using DotRecast.Recast.Toolset.Builder;
@@ -12,6 +14,9 @@ namespace DotRecast.Recast.Toolset.Tools
 {
     public class RcDynamicUpdateTool : IRcToolable
     {
+        private DynamicNavMesh dynaMesh;
+        private readonly Dictionary<long, RcGizmo> colliderGizmos;
+
         private readonly Random random;
         private readonly DemoInputGeomProvider bridgeGeom;
         private readonly DemoInputGeomProvider houseGeom;
@@ -19,29 +24,140 @@ namespace DotRecast.Recast.Toolset.Tools
 
         public RcDynamicUpdateTool(Random rand, DemoInputGeomProvider bridgeGeom, DemoInputGeomProvider houseGeom, DemoInputGeomProvider convexGeom)
         {
+            this.colliderGizmos = new Dictionary<long, RcGizmo>();
             this.random = rand;
             this.bridgeGeom = bridgeGeom;
             this.houseGeom = houseGeom;
             this.convexGeom = convexGeom;
         }
 
+        public IEnumerable<RcGizmo> GetGizmos()
+        {
+            return colliderGizmos.Values;
+        }
+
         public string GetName()
         {
             return "Dynamic Updates";
         }
 
-        public VoxelFile Load(string filename, IRcCompressor compressor)
+        public DynamicNavMesh GetDynamicNavMesh()
+        {
+            return dynaMesh;
+        }
+
+        public void RemoveShape(RcVec3f start, RcVec3f dir)
+        {
+            foreach (var e in colliderGizmos)
+            {
+                if (Hit(start, dir, e.Value.Collider.Bounds()))
+                {
+                    dynaMesh.RemoveCollider(e.Key);
+                    colliderGizmos.Remove(e.Key);
+                    break;
+                }
+            }
+        }
+
+        private bool Hit(RcVec3f point, RcVec3f dir, float[] bounds)
+        {
+            float cx = 0.5f * (bounds[0] + bounds[3]);
+            float cy = 0.5f * (bounds[1] + bounds[4]);
+            float cz = 0.5f * (bounds[2] + bounds[5]);
+            float dx = 0.5f * (bounds[3] - bounds[0]);
+            float dy = 0.5f * (bounds[4] - bounds[1]);
+            float dz = 0.5f * (bounds[5] - bounds[2]);
+            float rSqr = dx * dx + dy * dy + dz * dz;
+            float mx = point.x - cx;
+            float my = point.y - cy;
+            float mz = point.z - cz;
+            float c = mx * mx + my * my + mz * mz - rSqr;
+            if (c <= 0.0f)
+            {
+                return true;
+            }
+
+            float b = mx * dir.x + my * dir.y + mz * dir.z;
+            if (b > 0.0f)
+            {
+                return false;
+            }
+
+            float disc = b * b - c;
+            return disc >= 0.0f;
+        }
+
+
+        public RcGizmo AddShape(DynamicColliderShape colliderShape, RcVec3f p)
+        {
+            if (dynaMesh == null)
+            {
+                return null;
+            }
+
+            RcGizmo colliderWithGizmo = null;
+            {
+                if (colliderShape == DynamicColliderShape.SPHERE)
+                {
+                    colliderWithGizmo = SphereCollider(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.CAPSULE)
+                {
+                    colliderWithGizmo = CapsuleCollider(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.BOX)
+                {
+                    colliderWithGizmo = BoxCollider(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.CYLINDER)
+                {
+                    colliderWithGizmo = CylinderCollider(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.COMPOSITE)
+                {
+                    colliderWithGizmo = CompositeCollider(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.TRIMESH_BRIDGE)
+                {
+                    colliderWithGizmo = TrimeshBridge(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.TRIMESH_HOUSE)
+                {
+                    colliderWithGizmo = TrimeshHouse(p, dynaMesh.config.walkableClimb);
+                }
+                else if (colliderShape == DynamicColliderShape.CONVEX)
+                {
+                    colliderWithGizmo = ConvexTrimesh(p, dynaMesh.config.walkableClimb);
+                }
+            }
+
+            if (colliderWithGizmo != null)
+            {
+                long id = dynaMesh.AddCollider(colliderWithGizmo.Collider);
+                colliderGizmos.Add(id, colliderWithGizmo);
+            }
+
+            return colliderWithGizmo;
+        }
+
+        public DynamicNavMesh Load(string filename, IRcCompressor compressor)
         {
             using var fs = new FileStream(filename, FileMode.Open, FileAccess.Read);
             using var br = new BinaryReader(fs);
             VoxelFileReader reader = new VoxelFileReader(compressor);
             VoxelFile voxelFile = reader.Read(br);
 
-            return voxelFile;
+            dynaMesh = new DynamicNavMesh(voxelFile);
+            dynaMesh.config.keepIntermediateResults = true;
+
+            colliderGizmos.Clear();
+
+            return dynaMesh;
         }
 
-        public void Save(string filename, VoxelFile voxelFile, bool compression, IRcCompressor compressor)
+        public void Save(string filename, bool compression, IRcCompressor compressor)
         {
+            VoxelFile voxelFile = VoxelFile.From(dynaMesh);
             using var fs = new FileStream(filename, FileMode.CreateNew, FileAccess.Write);
             using var bw = new BinaryWriter(fs);
             VoxelFileWriter writer = new VoxelFileWriter(compressor);
@@ -122,7 +238,7 @@ namespace DotRecast.Recast.Toolset.Tools
                 SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb);
             var roofUp = RcVec3f.Zero;
             RcVec3f roofExtent = RcVec3f.Of(4.5f, 4.5f, 8f);
-            var rx = Matrix(45, forward.x, forward.y, forward.z);
+            var rx = RcMatrix4x4f.Rotate(45, forward.x, forward.y, forward.z);
             roofUp = MulMatrixVector(ref roofUp, rx, baseUp);
             RcVec3f roofCenter = RcVec3f.Of(p.x, p.y + 6, p.z);
             BoxCollider roof = new BoxCollider(roofCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(roofUp, forward, roofExtent),
@@ -181,8 +297,8 @@ namespace DotRecast.Recast.Toolset.Tools
 
         private float[] TransformVertices(RcVec3f p, DemoInputGeomProvider geom, float ax)
         {
-            var rx = Matrix((float)random.NextDouble() * ax, 1, 0, 0);
+            var rx = RcMatrix4x4f.Rotate((float)random.NextDouble() * ax, 1, 0, 0);
-            var ry = Matrix((float)random.NextDouble() * 360, 0, 1, 0);
+            var ry = RcMatrix4x4f.Rotate((float)random.NextDouble() * 360, 0, 1, 0);
             var m = RcMatrix4x4f.Mul(rx, ry);
             float[] verts = new float[geom.vertices.Length];
             RcVec3f v = new RcVec3f();
@@ -220,43 +336,33 @@ namespace DotRecast.Recast.Toolset.Tools
             return resultvector;
         }
 
-        public static RcMatrix4x4f Matrix(float a, float x, float y, float z)
+        public bool UpdateDynaMesh(TaskFactory executor)
         {
-            var matrix = new RcMatrix4x4f();
+            if (dynaMesh == null)
-            a = (float)(a * Math.PI / 180.0); // convert to radians
+            {
-            float s = (float)Math.Sin(a);
+                return false;
-            float c = (float)Math.Cos(a);
+            }
-            float t = 1.0f - c;
 
-            float tx = t * x;
+            bool updated = dynaMesh.Update(executor).Result;
-            float ty = t * y;
+            if (updated)
-            float tz = t * z;
+            {
+                return false;
+            }
 
-            float sz = s * z;
+            return true;
-            float sy = s * y;
+        }
-            float sx = s * x;
 
-            matrix.M11 = tx * x + c;
+        public bool Raycast(RcVec3f spos, RcVec3f epos, out float hitPos, out RcVec3f raycastHitPos)
-            matrix.M12 = tx * y + sz;
+        {
-            matrix.M13 = tx * z - sy;
+            RcVec3f sp = RcVec3f.Of(spos.x, spos.y + 1.3f, spos.z);
-            matrix.M14 = 0;
+            RcVec3f ep = RcVec3f.Of(epos.x, epos.y + 1.3f, epos.z);
 
-            matrix.M21 = tx * y - sz;
+            bool hasHit = dynaMesh.VoxelQuery().Raycast(sp, ep, out hitPos);
-            matrix.M22 = ty * y + c;
+            raycastHitPos = hasHit
-            matrix.M23 = ty * z + sx;
+                ? RcVec3f.Of(sp.x + hitPos * (ep.x - sp.x), sp.y + hitPos * (ep.y - sp.y), sp.z + hitPos * (ep.z - sp.z))
-            matrix.M24 = 0;
+                : ep;
 
-            matrix.M31 = tx * z + sy;
+            return hasHit;
-            matrix.M32 = ty * z - sx;
-            matrix.M33 = tz * z + c;
-            matrix.M34 = 0;
-
-            matrix.M41 = 0;
-            matrix.M42 = 0;
-            matrix.M43 = 0;
-            matrix.M44 = 1;
-
-            return matrix;
         }
     }
 }