From 419c52aaeb3d8ee55e1327516f7c0739e935e24d Mon Sep 17 00:00:00 2001 From: Andrew Gilewsky Date: Fri, 4 Oct 2024 23:11:55 +0100 Subject: [PATCH] Bitmap builder full implementation. --- vnavmesh/Debug/DebugNavmeshManager.cs | 57 +----------- vnavmesh/IPCProvider.cs | 1 + vnavmesh/NavmeshBitmap.cs | 120 +++++++++++++++++++------- vnavmesh/NavmeshManager.cs | 33 +++++++ 4 files changed, 125 insertions(+), 86 deletions(-) diff --git a/vnavmesh/Debug/DebugNavmeshManager.cs b/vnavmesh/Debug/DebugNavmeshManager.cs index 66a67fb..57e7c20 100644 --- a/vnavmesh/Debug/DebugNavmeshManager.cs +++ b/vnavmesh/Debug/DebugNavmeshManager.cs @@ -2,7 +2,6 @@ using Navmesh.Movement; using Navmesh.NavVolume; using System; -using System.IO; using System.Numerics; namespace Navmesh.Debug; @@ -113,61 +112,7 @@ private void DrawPosition(string tag, Vector3 position) private void ExportBitmap(Navmesh navmesh, NavmeshQuery query, Vector3 startingPos) { - 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); - wr.Write((ushort)0x4D42); // 'BM' (word) - wr.Write(14 + 12 + 8 + bitmap.Data.Length); // file size (dword) - wr.Write(0); // reserved (2x word) - wr.Write(14 + 12 + 8); // pixel data offset (dword) - wr.Write(12); // BITMAPCOREHEADER size (dword) - wr.Write((ushort)bitmap.Width); // width in pixels (word) - wr.Write((ushort)bitmap.Height); // height in pixels (word) - wr.Write((ushort)1); // num color planes (word) - wr.Write((ushort)1); // bits per pixel (word) - wr.Write(0x00ffff00); // color 0 - wr.Write(0x00000000); // color 1 - wr.Write(bitmap.Data); // pixel data + _manager.BuildBitmap(startingPos, "D:\\navmesh.bmp", 0.5f); } private void OnNavmeshChanged(Navmesh? navmesh, NavmeshQuery? query) diff --git a/vnavmesh/IPCProvider.cs b/vnavmesh/IPCProvider.cs index f6ed19e..e102dcd 100644 --- a/vnavmesh/IPCProvider.cs +++ b/vnavmesh/IPCProvider.cs @@ -23,6 +23,7 @@ public IPCProvider(NavmeshManager navmeshManager, FollowPath followPath, AsyncMo RegisterFunc("Nav.PathfindNumQueued", () => navmeshManager.NumQueuedPathfindRequests); RegisterFunc("Nav.IsAutoLoad", () => Service.Config.AutoLoadNavmesh); RegisterAction("Nav.SetAutoLoad", (bool v) => { Service.Config.AutoLoadNavmesh = v; Service.Config.NotifyModified(); }); + RegisterFunc("Nav.BuildBitmap", (Vector3 startingPos, string filename, float pixelSize) => navmeshManager.BuildBitmap(startingPos, filename, pixelSize)); RegisterFunc("Query.Mesh.NearestPoint", (Vector3 p, float halfExtentXZ, float halfExtentY) => navmeshManager.Query?.FindNearestPointOnMesh(p, halfExtentXZ, halfExtentY)); RegisterFunc("Query.Mesh.PointOnFloor", (Vector3 p, bool allowUnlandable, float halfExtentXZ) => navmeshManager.Query?.FindPointOnFloor(p, halfExtentXZ)); diff --git a/vnavmesh/NavmeshBitmap.cs b/vnavmesh/NavmeshBitmap.cs index 5f3a561..8846cc2 100644 --- a/vnavmesh/NavmeshBitmap.cs +++ b/vnavmesh/NavmeshBitmap.cs @@ -1,29 +1,80 @@ using DotRecast.Detour; +using SharpDX.Win32; using System; +using System.IO; using System.Numerics; +using System.Runtime.InteropServices; namespace Navmesh; -public class NavmeshBitmap +// this is all stolen from vbm - utility for working with 2d 1bpp bitmaps +// some notes: +// - supports only BITMAPINFOHEADER (could've been BITMAPCOREHEADER, but bottom-up bitmaps don't make sense with FF coordinate system) +// - supports only 1bpp bitmaps without compression; per bitmap spec, first pixel is highest bit, etc. +// - supports only top-down bitmaps (with negative height) +// - horizontal/vertical resolution is equal and is 'pixels per 1024 world units' +// - per bitmap spec, rows are padded to 4 byte alignment +public sealed class NavmeshBitmap { - public Vector3 MinBounds; - public Vector3 MaxBounds; - public float Resolution; - public float InvResolution; - public int Width; - public int Height; - public byte[] Data; // 1 if walkable - - public NavmeshBitmap(Vector3 min, Vector3 max, float resolution) + [StructLayout(LayoutKind.Explicit, Size = 14)] + public struct FileHeader { - 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]; + [FieldOffset(0)] public ushort Type; // 0x4D42 'BM' + [FieldOffset(2)] public int Size; // size of the file in bytes + [FieldOffset(6)] public uint Reserved; + [FieldOffset(10)] public int OffBits; // offset from this to pixel data + } + public const ushort Magic = 0x4D42; + + public readonly float PixelSize; + public readonly float Resolution; + public readonly Vector3 MinBounds; + public readonly Vector3 MaxBounds; + public readonly int Width; + public readonly int Height; + public readonly int BytesPerRow; + public readonly byte[] Pixels; // 1 if unwalkable + + public int CoordToIndex(int x, int y) => y * BytesPerRow + (x >> 3); + public byte CoordToMask(int x) => (byte)(0x80u >> (x & 7)); + public ref byte ByteAt(int x, int y) => ref Pixels[CoordToIndex(x, y)]; + + public bool this[int x, int y] + { + get => (ByteAt(x, y) & CoordToMask(x)) != 0; + set + { + if (value) + ByteAt(x, y) |= CoordToMask(x); + else + ByteAt(x, y) &= (byte)~CoordToMask(x); + } + } + + // note: pixelSize should be power-of-2 + public NavmeshBitmap(Vector3 min, Vector3 max, float pixelSize) + { + PixelSize = pixelSize; + Resolution = 1.0f / pixelSize; + MinBounds = (min * Resolution).Floor() * PixelSize; + MaxBounds = (max * Resolution).Ceiling() * PixelSize; + Width = (int)((max.X - min.X) * Resolution); + Height = (int)((max.Z - min.Z) * Resolution); + BytesPerRow = (Width + 31) >> 5 << 2; + Pixels = new byte[Height * BytesPerRow]; + Array.Fill(Pixels, (byte)0xFF); + } + + public void Save(string filename) + { + var intRes = (int)(1024 * Resolution); + using var fstream = new FileStream(filename, FileMode.Create, FileAccess.Write, FileShare.Read); + var headerSize = Marshal.SizeOf() + Marshal.SizeOf() + 2 * Marshal.SizeOf(); + WriteStruct(fstream, new FileHeader() { Type = Magic, Size = headerSize + Pixels.Length, OffBits = headerSize }); + WriteStruct(fstream, new BitmapInfoHeader() { SizeInBytes = Marshal.SizeOf(), Width = Width, Height = -Height, PlaneCount = 1, BitCount = 1, XPixelsPerMeter = intRes, YPixelsPerMeter = intRes }); + WriteStruct(fstream, 0u); + WriteStruct(fstream, 0xffff7f00u); + fstream.Write(Pixels); } public void RasterizePolygon(DtNavMesh mesh, long poly) @@ -51,30 +102,26 @@ public void RasterizePolygon(DtMeshTile tile, DtPoly poly) for (int i = 1; i < poly.vertCount; ++i) edges[i] = verts[i - 1] - verts[i]; - int x0 = Math.Clamp((int)MathF.Floor((min.X - MinBounds.X) * InvResolution), 0, Width - 1); - 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); + int x0 = Math.Clamp((int)MathF.Floor((min.X - MinBounds.X) * Resolution), 0, Width - 1); + int z0 = Math.Clamp((int)MathF.Floor((min.Z - MinBounds.Z) * Resolution), 0, Height - 1); + int x1 = Math.Clamp((int)MathF.Ceiling((max.X - MinBounds.X) * Resolution), 0, Width - 1); + int z1 = Math.Clamp((int)MathF.Ceiling((max.Z - MinBounds.Z) * Resolution), 0, Height - 1); //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); - var iz = (Height - 1 - z0) * Width + x0; // TODO: z0 * Width to remove z inversion + Vector2 cz = new(MinBounds.X + (x0 + 0.5f) * PixelSize, MinBounds.Z + (z0 + 0.5f) * PixelSize); for (int z = z0; z <= z1; ++z) { var cx = cz; - var ix = iz; for (int x = x0; x <= x1; ++x) { var inside = PointInPolygon(verts, edges, cx); //Service.Log.Debug($"test {x},{z} ({cx}) = {inside}"); if (inside) - Data[ix >> 3] |= (byte)(0x80 >> (ix & 7)); - ++ix; - cx.X += Resolution; + this[x, z] = false; + cx.X += PixelSize; } - iz -= Width; // TODO += to remove z inversion - cz.Y += Resolution; + cz.Y += PixelSize; } } @@ -96,4 +143,17 @@ private static bool PointInPolygon(ReadOnlySpan verts, ReadOnlySpan(Stream stream) where T : unmanaged + { + T res = default; + stream.Read(new(&res, sizeof(T))); + return res; + } + + public static unsafe void WriteStruct(Stream stream, in T value) where T : unmanaged + { + fixed (T* ptr = &value) + stream.Write(new(ptr, sizeof(T))); + } } diff --git a/vnavmesh/NavmeshManager.cs b/vnavmesh/NavmeshManager.cs index 0ec31fb..0bf2569 100644 --- a/vnavmesh/NavmeshManager.cs +++ b/vnavmesh/NavmeshManager.cs @@ -162,6 +162,39 @@ public void CancelAllQueries() _queryCancelSource = new(); // create new token source for future tasks } + // note: pixelSize should be power-of-2 + public (Vector3 min, Vector3 max) BuildBitmap(Vector3 startingPos, string filename, float pixelSize) + { + if (Navmesh == null || Query == null) + throw new InvalidOperationException($"Can't build bitmap - navmesh creation is in progress"); + + 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, poly.verts[i]); + min = Vector3.Min(min, v); + max = Vector3.Max(max, v); + //Service.Log.Debug($"{p:X}.{i}= {v}"); + } + } + //Service.Log.Debug($"bounds: {min}-{max}"); + + var bitmap = new NavmeshBitmap(min, max, pixelSize); + foreach (var p in reachablePolys) + { + bitmap.RasterizePolygon(Navmesh.Mesh, p); + } + bitmap.Save(filename); + Service.Log.Debug($"Generated nav bitmap '{filename}' @ {startingPos}: {bitmap.MinBounds}-{bitmap.MaxBounds}"); + return (bitmap.MinBounds, bitmap.MaxBounds); + } + // if non-empty string is returned, active layout is ready private unsafe string GetCurrentKey() {