muffin v7.5.13

SmartNav.Model/SmartNav.Model.Navigation/TerritoryWaterMap.cs

@@ -0,0 +1,188 @@
+using System;
+using System.Numerics;
+
+namespace SmartNav.Model.Navigation;
+
+public sealed class TerritoryWaterMap
+{
+	public const int GridSize = 256;
+
+	public const int CellCount = 65536;
+
+	public const float CellSize = 8f;
+
+	public const float WorldMin = -1024f;
+
+	public const float WorldMax = 1024f;
+
+	private const float SurfaceYScale = 32f;
+
+	private const float SurfaceYBias = 1024f;
+
+	private readonly byte[] _classes;
+
+	private readonly ushort[] _surfaceY;
+
+	public ushort TerritoryId { get; }
+
+	public ReadOnlySpan<byte> Classes => _classes;
+
+	public ReadOnlySpan<ushort> SurfaceYRaw => _surfaceY;
+
+	public bool HasWater => ((ReadOnlySpan<byte>)_classes.AsSpan()).ContainsAnyExcept((byte)0);
+
+	public TerritoryWaterMap(ushort territoryId)
+		: this(territoryId, new byte[65536], new ushort[65536])
+	{
+	}
+
+	public TerritoryWaterMap(ushort territoryId, byte[] classes, ushort[] surfaceY)
+	{
+		ArgumentNullException.ThrowIfNull(classes, "classes");
+		ArgumentNullException.ThrowIfNull(surfaceY, "surfaceY");
+		if (classes.Length != 65536)
+		{
+			throw new ArgumentException($"expected {65536} class entries, got {classes.Length}", "classes");
+		}
+		if (surfaceY.Length != 65536)
+		{
+			throw new ArgumentException($"expected {65536} surface Y entries, got {surfaceY.Length}", "surfaceY");
+		}
+		TerritoryId = territoryId;
+		_classes = classes;
+		_surfaceY = surfaceY;
+	}
+
+	public static ushort EncodeSurfaceY(float y)
+	{
+		if (float.IsNaN(y))
+		{
+			throw new ArgumentException("surface Y is NaN", "y");
+		}
+		float num = MathF.Round((y + 1024f) * 32f);
+		if (num <= 0f)
+		{
+			return 0;
+		}
+		if (num >= 65535f)
+		{
+			return ushort.MaxValue;
+		}
+		return (ushort)num;
+	}
+
+	public static float DecodeSurfaceY(ushort raw)
+	{
+		return (float)(int)raw / 32f - 1024f;
+	}
+
+	public static float CellCenterCoordinate(int colOrRow)
+	{
+		return -1024f + ((float)colOrRow + 0.5f) * 8f;
+	}
+
+	public static bool TryGetCell(float x, float z, out int col, out int row)
+	{
+		col = (int)MathF.Floor((x - -1024f) / 8f);
+		row = (int)MathF.Floor((z - -1024f) / 8f);
+		if (col < 0 || col >= 256 || row < 0 || row >= 256)
+		{
+			col = -1;
+			row = -1;
+			return false;
+		}
+		return true;
+	}
+
+	public void SetCell(int col, int row, WaterCellClass cellClass, float surfaceY)
+	{
+		int num = IndexOf(col, row);
+		_classes[num] = (byte)cellClass;
+		_surfaceY[num] = EncodeSurfaceY(surfaceY);
+	}
+
+	public WaterCellClass GetCell(int col, int row)
+	{
+		return (WaterCellClass)_classes[IndexOf(col, row)];
+	}
+
+	public WaterCellClass ClassifyAt(float x, float z)
+	{
+		if (!TryGetCell(x, z, out var col, out var row))
+		{
+			return WaterCellClass.None;
+		}
+		return (WaterCellClass)_classes[row * 256 + col];
+	}
+
+	public bool TryGetSurfaceY(float x, float z, out float surfaceY)
+	{
+		surfaceY = 0f;
+		if (!TryGetCell(x, z, out var col, out var row))
+		{
+			return false;
+		}
+		int num = row * 256 + col;
+		if (_classes[num] == 0)
+		{
+			return false;
+		}
+		surfaceY = DecodeSurfaceY(_surfaceY[num]);
+		return true;
+	}
+
+	public bool TryFindNearestDivable(float x, float z, float maxRadiusYalms, out Vector3 entryCellCenter)
+	{
+		entryCellCenter = default(Vector3);
+		if (float.IsNaN(maxRadiusYalms) || maxRadiusYalms < 0f || !TryGetCell(x, z, out var col, out var row))
+		{
+			return false;
+		}
+		float num = MathF.Ceiling(maxRadiusYalms / 8f) + 1f;
+		int num2 = ((num >= 256f) ? 256 : ((int)num));
+		float num3 = float.MaxValue;
+		int num4 = -1;
+		int num5 = -1;
+		for (int i = 0; i <= num2 && (num4 < 0 || !(((float)i - 0.5f) * 8f > num3)); i++)
+		{
+			int num6 = col - i;
+			int num7 = col + i;
+			int num8 = row - i;
+			int num9 = row + i;
+			for (int j = Math.Max(num8, 0); j <= Math.Min(num9, 255); j++)
+			{
+				int num10 = ((j == num8 || j == num9) ? 1 : Math.Max(num7 - num6, 1));
+				for (int k = num6; k <= num7; k += num10)
+				{
+					if (k >= 0 && k < 256 && _classes[j * 256 + k] == 2)
+					{
+						float num11 = CellCenterCoordinate(k) - x;
+						float num12 = CellCenterCoordinate(j) - z;
+						float num13 = MathF.Sqrt(num11 * num11 + num12 * num12);
+						if (!(num13 > maxRadiusYalms) && !(num13 >= num3))
+						{
+							num3 = num13;
+							num4 = k;
+							num5 = j;
+						}
+					}
+				}
+			}
+		}
+		if (num4 < 0)
+		{
+			return false;
+		}
+		entryCellCenter = new Vector3(CellCenterCoordinate(num4), DecodeSurfaceY(_surfaceY[num5 * 256 + num4]), CellCenterCoordinate(num5));
+		return true;
+	}
+
+	private static int IndexOf(int col, int row)
+	{
+		ArgumentOutOfRangeException.ThrowIfNegative(col, "col");
+		ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(col, 256, "col");
+		ArgumentOutOfRangeException.ThrowIfNegative(row, "row");
+		ArgumentOutOfRangeException.ThrowIfGreaterThanOrEqual(row, 256, "row");
+		return row * 256 + col;
+	}
+}