Skip to content

Commit

Permalink
Bitmap builder full implementation.
Browse files Browse the repository at this point in the history
  • Loading branch information
@awgil
awgil committed Oct 4, 2024
1 parent 1142997 commit 419c52a
Show file tree
Hide file tree
Showing 4 changed files with 125 additions and 86 deletions.
57 changes: 1 addition & 56 deletions vnavmesh/Debug/DebugNavmeshManager.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
using Navmesh.Movement;
using Navmesh.NavVolume;
using System;
using System.IO;
using System.Numerics;

namespace Navmesh.Debug;
Expand Down Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions vnavmesh/IPCProvider.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
120 changes: 90 additions & 30 deletions vnavmesh/NavmeshBitmap.cs
View file
Original file line number Diff line number Diff line change
@@ -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<FileHeader>() + Marshal.SizeOf<BitmapInfoHeader>() + 2 * Marshal.SizeOf<uint>();
WriteStruct(fstream, new FileHeader() { Type = Magic, Size = headerSize + Pixels.Length, OffBits = headerSize });
WriteStruct(fstream, new BitmapInfoHeader() { SizeInBytes = Marshal.SizeOf<BitmapInfoHeader>(), 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)
Expand Down Expand Up @@ -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;
}
}

Expand All @@ -96,4 +143,17 @@ private static bool PointInPolygon(ReadOnlySpan<Vector2> verts, ReadOnlySpan<Vec
}
return true;
}

public static unsafe T ReadStruct<T>(Stream stream) where T : unmanaged
{
T res = default;
stream.Read(new(&res, sizeof(T)));
return res;
}

public static unsafe void WriteStruct<T>(Stream stream, in T value) where T : unmanaged
{
fixed (T* ptr = &value)
stream.Write(new(ptr, sizeof(T)));
}
}
33 changes: 33 additions & 0 deletions vnavmesh/NavmeshManager.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down

0 comments on commit 419c52a

Please sign in to comment.