diff --git a/vnavmesh/Debug/DebugNavmeshManager.cs b/vnavmesh/Debug/DebugNavmeshManager.cs index 37d0389..66a67fb 100644 --- a/vnavmesh/Debug/DebugNavmeshManager.cs +++ b/vnavmesh/Debug/DebugNavmeshManager.cs @@ -17,7 +17,6 @@ class DebugNavmeshManager : IDisposable private DebugDrawer _dd; private DebugGameCollision _coll; private Vector3 _target; - private Vector2 _bitmapBounds = new(-1024, 1024); private DebugDetourNavmesh? _drawNavmesh; private DebugVoxelMap? _debugVoxelMap; @@ -77,10 +76,8 @@ public void Draw() ImGui.SameLine(); ImGui.TextUnformatted($"Current target: {_target}"); - ImGui.DragFloatRange2("Vertical bounds", ref _bitmapBounds.X, ref _bitmapBounds.Y, 0.1f, -1024, 1024); - ImGui.SameLine(); if (ImGui.Button("Export bitmap")) - ExportBitmap(_manager.Navmesh); + ExportBitmap(_manager.Navmesh, _manager.Query, playerPos); ImGui.Checkbox("Allow movement", ref _path.MovementAllowed); ImGui.Checkbox("Use raycasts", ref _manager.UseRaycasts); @@ -114,9 +111,48 @@ private void DrawPosition(string tag, Vector3 position) _debugVoxelMap?.VisualizeVoxel(voxel); } - private void ExportBitmap(Navmesh navmesh) + private void ExportBitmap(Navmesh navmesh, NavmeshQuery query, Vector3 startingPos) { - var bitmap = new NavmeshBitmap(navmesh, new(-1024, _bitmapBounds.X, -1024), new(1024, _bitmapBounds.Y, 1024), 0.5f); + var startPoly = query.FindNearestMeshPoly(startingPos); + var reachablePolys = query.FindReachableMeshPolys(startPoly); + + Vector3 min = new(1024), max = new(-1024); + foreach (var p in reachablePolys) + { + navmesh.Mesh.GetTileAndPolyByRefUnsafe(p, out var tile, out var poly); + for (int i = 0; i < poly.vertCount; ++i) + { + var v = NavmeshBitmap.GetVertex(tile, i); + min = Vector3.Min(min, v); + max = Vector3.Max(max, v); + } + } + + var bitmap = new NavmeshBitmap(min, max, 0.5f); + foreach (var p in reachablePolys) + { + bitmap.RasterizePolygon(navmesh.Mesh, p); + } + //for (int i = 0, numTiles = navmesh.Mesh.GetParams().maxTiles; i < numTiles; ++i) + //{ + // //if (i != 9) + // // continue; + // var tile = navmesh.Mesh.GetTile(i); + // if (tile.data == null) + // continue; + + // for (int j = 0; j < tile.data.header.polyCount; ++j) + // { + // //if (j != 583) + // // continue; + // var p = tile.data.polys[j]; + // if (p.GetPolyType() != DtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION && p.vertCount >= 3) + // { + // bitmap.RasterizePolygon(tile, p); + // } + // } + //} + //var bitmap = new NavmeshBitmap(navmesh, new(-128, 10, -128), new(0, 30, 0), 0.5f); using var fs = new FileStream("D:\\navmesh.bmp", FileMode.Create, FileAccess.Write); using var wr = new BinaryWriter(fs); diff --git a/vnavmesh/NavmeshBitmap.cs b/vnavmesh/NavmeshBitmap.cs index 6d0d997..5f3a561 100644 --- a/vnavmesh/NavmeshBitmap.cs +++ b/vnavmesh/NavmeshBitmap.cs @@ -14,29 +14,25 @@ public class NavmeshBitmap public int Height; public byte[] Data; // 1 if walkable - private Vector3 GetVertex(DtMeshTile tile, int i) => new(tile.data.verts[i * 3], tile.data.verts[i * 3 + 1], tile.data.verts[i * 3 + 2]); - - private static float Cross(Vector2 a, Vector2 b) => a.X * b.Y - a.Y * b.X; + public NavmeshBitmap(Vector3 min, Vector3 max, float resolution) + { + MinBounds = min; + MaxBounds = max; + Resolution = resolution; + InvResolution = 1 / resolution; + Width = (int)MathF.Ceiling((max.X - min.X) * InvResolution); + Width = (Width + 31) & ~31; // round up to multiple of 32 + Height = (int)MathF.Ceiling((max.Z - min.Z) * InvResolution); + Data = new byte[Width * Height >> 3]; + } - private static bool PointInPolygon(ReadOnlySpan verts, ReadOnlySpan edges, Vector2 p) + public void RasterizePolygon(DtNavMesh mesh, long poly) { - var orient = Cross(p - verts[0], edges[0]); - //Service.Log.Debug($"base {p} x {verts[0]} x {verts[^1]} = {orient}"); - if (orient == 0) - return true; - for (int i = 1; i < verts.Length; ++i) - { - var cur = Cross(p - verts[i], edges[i]); - //Service.Log.Debug($"oth {p} x {verts[i]} x {verts[i - 1]} = {cur}"); - if (cur == 0) - return true; - if (cur * orient < 1) - return false; - } - return true; + mesh.GetTileAndPolyByRefUnsafe(poly, out var t, out var p); + RasterizePolygon(t, p); } - private void RasterizePolygon(DtMeshTile tile, DtPoly poly) + public void RasterizePolygon(DtMeshTile tile, DtPoly poly) { Vector3 min = new(float.MaxValue), max = new(float.MinValue); Span verts = stackalloc Vector2[poly.vertCount]; @@ -59,7 +55,7 @@ private void RasterizePolygon(DtMeshTile tile, DtPoly poly) int z0 = Math.Clamp((int)MathF.Floor((min.Z - MinBounds.Z) * InvResolution), 0, Height - 1); int x1 = Math.Clamp((int)MathF.Ceiling((max.X - MinBounds.X) * InvResolution), 0, Width - 1); int z1 = Math.Clamp((int)MathF.Ceiling((max.Z - MinBounds.Z) * InvResolution), 0, Height - 1); - //Service.Log.Debug($"{x0},{z0} - {x1},{z1}"); + //Service.Log.Debug($"{x0},{z0} - {x1},{z1} ({min}-{max} vs {MinBounds}-{MaxBounds})"); //for (int i = 0; i < poly.vertCount; ++i) // Service.Log.Debug($"[{i}] {verts[i]} ({edges[i]})"); Vector2 cz = new(MinBounds.X + (x0 + 0.5f) * Resolution, MinBounds.Z + (z0 + 0.5f) * Resolution); @@ -73,7 +69,7 @@ private void RasterizePolygon(DtMeshTile tile, DtPoly poly) var inside = PointInPolygon(verts, edges, cx); //Service.Log.Debug($"test {x},{z} ({cx}) = {inside}"); if (inside) - Data[ix >> 3] |= (byte)(1 << (ix & 7)); + Data[ix >> 3] |= (byte)(0x80 >> (ix & 7)); ++ix; cx.X += Resolution; } @@ -82,34 +78,22 @@ private void RasterizePolygon(DtMeshTile tile, DtPoly poly) } } - public NavmeshBitmap(Navmesh mesh, Vector3 min, Vector3 max, float resolution) + public static Vector3 GetVertex(DtMeshTile tile, int i) => new(tile.data.verts[i * 3], tile.data.verts[i * 3 + 1], tile.data.verts[i * 3 + 2]); + + private static float Cross(Vector2 a, Vector2 b) => a.X * b.Y - a.Y * b.X; + + private static bool PointInPolygon(ReadOnlySpan verts, ReadOnlySpan edges, Vector2 p) { - MinBounds = min; - MaxBounds = max; - Resolution = resolution; - InvResolution = 1 / resolution; - Width = (int)MathF.Ceiling((max.X - min.X) * InvResolution); - Width = (Width + 31) & ~31; // round up to multiple of 32 - Height = (int)MathF.Ceiling((max.Z - min.Z) * InvResolution); - Data = new byte[Width * Height >> 3]; - for (int i = 0, numTiles = mesh.Mesh.GetParams().maxTiles; i < numTiles; ++i) + float orient = 0; + for (int i = 0; i < verts.Length; ++i) { - //if (i != 9) - // continue; - var tile = mesh.Mesh.GetTile(i); - if (tile.data == null) - continue; - - for (int j = 0; j < tile.data.header.polyCount; ++j) - { - //if (j != 583) - // continue; - var p = tile.data.polys[j]; - if (p.GetPolyType() != DtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION && p.vertCount >= 3) - { - RasterizePolygon(tile, p); - } - } + var cur = Cross(p - verts[i], edges[i]); + //Service.Log.Debug($"> {p} x {verts[i]} x {edges[i]} = {cur}"); + if (orient == 0) + orient = cur; + else if ((cur > 0 && orient < 0) || (cur < 0 && orient > 0)) + return false; } + return true; } } diff --git a/vnavmesh/NavmeshQuery.cs b/vnavmesh/NavmeshQuery.cs index 6453113..6b71581 100644 --- a/vnavmesh/NavmeshQuery.cs +++ b/vnavmesh/NavmeshQuery.cs @@ -131,4 +131,32 @@ public List FindIntersectingMeshPolys(Vector3 p, Vector3 halfExtent) // returns VoxelMap.InvalidVoxel if not found, otherwise voxel index public ulong FindNearestVolumeVoxel(Vector3 p, float halfExtentXZ = 5, float halfExtentY = 5) => VolumeQuery != null ? VoxelSearch.FindNearestEmptyVoxel(VolumeQuery.Volume, p, new(halfExtentXZ, halfExtentY, halfExtentXZ)) : VoxelMap.InvalidVoxel; + + // collect all mesh polygons reachable from specified polygon + public HashSet FindReachableMeshPolys(long starting) + { + HashSet result = []; + if (starting == 0) + return result; + + List queue = [starting]; + while (queue.Count > 0) + { + var next = queue[^1]; + queue.RemoveAt(queue.Count - 1); + + if (!result.Add(next)) + continue; // already visited + + MeshQuery.GetAttachedNavMesh().GetTileAndPolyByRefUnsafe(next, out var nextTile, out var nextPoly); + for (int i = nextTile.polyLinks[nextPoly.index]; i != DtNavMesh.DT_NULL_LINK; i = nextTile.links[i].next) + { + long neighbourRef = nextTile.links[i].refs; + if (neighbourRef != 0) + queue.Add(neighbourRef); + } + } + + return result; + } }