Skip to content

Commit

Permalink
Bugfixes.
Browse files Browse the repository at this point in the history
  • Loading branch information
@awgil
awgil committed Sep 30, 2024
1 parent e2de7bf commit 1142997
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 53 deletions.
48 changes: 42 additions & 6 deletions vnavmesh/Debug/DebugNavmeshManager.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
78 changes: 31 additions & 47 deletions vnavmesh/NavmeshBitmap.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Vector2> verts, ReadOnlySpan<Vector2> 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<Vector2> verts = stackalloc Vector2[poly.vertCount];
Expand All @@ -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);
Expand All @@ -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;
}
Expand All @@ -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<Vector2> verts, ReadOnlySpan<Vector2> 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;
}
}
28 changes: 28 additions & 0 deletions vnavmesh/NavmeshQuery.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,4 +131,32 @@ public List<long> 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<long> FindReachableMeshPolys(long starting)
{
HashSet<long> result = [];
if (starting == 0)
return result;

List<long> 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;
}
}

0 comments on commit 1142997

Please sign in to comment.