RailGeometry.cs

using System.Numerics;

namespace RustMapViewer;

public static class RailGeometry
{
	public static List<WorldFile.Vec> BuildRawSpline(WorldFile.Path path, float sampleStep)
	{
		Vector3[] points = ToPoints(path);
		bool circular = Vector3.Distance(points[0], points[^1]) < 0.1f;
		return Sample(points, circular, sampleStep);
	}

	public static List<WorldFile.Vec> BuildClientSpline(WorldFile.Path path, HeightMap height, float sampleStep, int iterations = 64, bool endFilter = true)
	{
		Vector3[] points = ToPoints(path);
		bool circular = Vector3.Distance(points[0], points[^1]) < 0.1f;
		int maxIndex = points.Length - 1;

		for (int i = 1; i < points.Length - 1; i++)
		{
			points[i].Y = MathF.Max(height.GetHeight(points[i].X, points[i].Z), 1f);
		}

		Smoothen(points, circular, iterations, endFilter ? i => Filter(i, maxIndex) : _ => 1f);
		return Sample(points, circular, sampleStep);
	}

	private static Vector3[] ToPoints(WorldFile.Path path)
	{
		Vector3[] points = new Vector3[path.Nodes.Count];
		for (int i = 0; i < points.Length; i++)
		{
			points[i] = new Vector3(path.Nodes[i].X, path.Nodes[i].Y, path.Nodes[i].Z);
		}
		return points;
	}

	private static List<WorldFile.Vec> Sample(Vector3[] points, bool circular, float sampleStep)
	{
		Vector3[] tangents = RecalculateTangents(points, circular);
		float length = 0f;
		for (int i = 0; i < points.Length - 1; i++)
		{
			length += Vector3.Distance(points[i], points[i + 1]);
		}
		float stepSize = length / points.Length;

		int sampleCount = Math.Max(2, (int)(length / sampleStep));
		List<WorldFile.Vec> samples = new(sampleCount + 1);
		for (int i = 0; i <= sampleCount; i++)
		{
			Vector3 p = GetPointCubicHermite(points, tangents, stepSize, length, length * i / sampleCount);
			samples.Add(new WorldFile.Vec(p.X, p.Y, p.Z));
		}
		return samples;
	}

	public static void ApplyBerm(HeightMap height, List<WorldFile.Vec> spline)
	{
		const float bedHalfWidth = 1.9f;
		const float bedTopHalfWidth = 1.3f;
		const float bedTopHeight = 0.26f;

		for (int i = 1; i < spline.Count; i++)
		{
			WorldFile.Vec a = spline[i - 1];
			WorldFile.Vec b = spline[i];
			float dx = b.X - a.X;
			float dz = b.Z - a.Z;
			float segLenSq = dx * dx + dz * dz;
			if (segLenSq < 0.0001f)
			{
				continue;
			}

			int x0 = height.WorldToIdxX(MathF.Min(a.X, b.X) - bedHalfWidth);
			int x1 = height.WorldToIdxX(MathF.Max(a.X, b.X) + bedHalfWidth);
			int z0 = height.WorldToIdxZ(MathF.Min(a.Z, b.Z) - bedHalfWidth);
			int z1 = height.WorldToIdxZ(MathF.Max(a.Z, b.Z) + bedHalfWidth);

			for (int z = z0; z <= z1; z++)
			{
				for (int x = x0; x <= x1; x++)
				{
					(float wx, float wz) = height.IdxToWorld(x, z);
					float t = Math.Clamp(((wx - a.X) * dx + (wz - a.Z) * dz) / segLenSq, 0f, 1f);
					float px = a.X + dx * t;
					float pz = a.Z + dz * t;
					float dist = MathF.Sqrt((wx - px) * (wx - px) + (wz - pz) * (wz - pz));
					if (dist > bedHalfWidth)
					{
						continue;
					}

					float profile = dist <= bedTopHalfWidth
						? bedTopHeight
						: bedTopHeight * (bedHalfWidth - dist) / (bedHalfWidth - bedTopHalfWidth);
					height.RaiseIdx(x, z, a.Y + (b.Y - a.Y) * t + profile);
				}
			}
		}
	}

	private static float Filter(int i, int maxIndex)
	{
		float a = Math.Clamp(i / 8f, 0f, 1f);
		float b = Math.Clamp((maxIndex - i) / 8f, 0f, 1f);
		float t = MathF.Min(a, b);
		return t * t * (3f - 2f * t);
	}

	private static void Smoothen(Vector3[] points, bool circular, int iterations, Func<int, float> filter)
	{
		int maxIndex = points.Length - 1;
		for (int iteration = 0; iteration < iterations; iteration++)
		{
			for (int j = circular ? 0 : 1; j <= maxIndex - 1; j += 2)
			{
				SmoothenIndex(points, j, filter);
			}
			for (int k = circular ? 1 : 2; k <= maxIndex - 1; k += 2)
			{
				SmoothenIndex(points, k, filter);
			}
		}
	}

	private static void SmoothenIndex(Vector3[] points, int i, Func<int, float> filter)
	{
		int prev = i - 1;
		int next = i + 1;
		if (i == 0)
		{
			prev = points.Length - 2;
		}

		Vector3 p = points[i];
		float smoothedY = (points[prev].Y + p.Y + p.Y + points[next].Y) * 0.25f;
		float multiplier = filter(i);
		points[i].Y = p.Y + (smoothedY - p.Y) * multiplier;

		if (i == 0)
		{
			points[^1] = points[0];
		}
	}

	private static Vector3[] RecalculateTangents(Vector3[] points, bool circular)
	{
		Vector3[] tangents = new Vector3[points.Length];
		for (int i = 0; i < points.Length; i++)
		{
			int prev = i - 1;
			int next = i + 1;
			if (prev < 0)
			{
				prev = circular ? points.Length - 2 : 0;
			}
			if (next > points.Length - 1)
			{
				next = circular ? 1 : points.Length - 1;
			}
			tangents[i] = Vector3.Normalize(points[next] - points[prev]);
		}
		return tangents;
	}

	private static Vector3 GetPointCubicHermite(Vector3[] points, Vector3[] tangents, float stepSize, float length, float distance)
	{
		if (length <= 0f)
		{
			return points[0];
		}

		float f = distance / length * (points.Length - 1);
		int index = (int)f;
		if (f <= 0f)
		{
			return points[0];
		}
		if (f >= points.Length - 1)
		{
			return points[^1];
		}

		Vector3 p0 = points[index];
		Vector3 p1 = points[index + 1];
		Vector3 m0 = tangents[index] * stepSize;
		Vector3 m1 = tangents[index + 1] * stepSize;
		float t = f - index;
		float t2 = t * t;
		float t3 = t * t2;
		return (2f * t3 - 3f * t2 + 1f) * p0 + (t3 - 2f * t2 + t) * m0 + (-2f * t3 + 3f * t2) * p1 + (t3 - t2) * m1;
	}
}