sbox-public/engine/Sandbox.System/Math/Spline.Utility.cs

using System.Collections.ObjectModel;
using System.Runtime.InteropServices;


namespace Sandbox;

// Immutable and stateless spline calculations and utilities.
// Will be used by higher level abstractions such as the SplineComponent
// Note: We probably wont want to expose this until the interface is stable
// and we know exactly which of these functions are actually needed/desired
internal static class SplineUtils
{
	public struct SegmentParams
	{
		public int Index;

		public float T;
	}

	public static Vector3 GetPosition( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );

		// Calculate value for cubic Bernstein polynominal
		// B(t) = (1 - t)^3 * P0 + 3 * (1 - t)^2 * t * P1 + 3 * (1 - t)^2 * t^2 * P2 + t^3 * P3
		float tSquare = segmentParams.T * segmentParams.T;
		float tCubic = tSquare * segmentParams.T;
		float oneMinusT = 1 - segmentParams.T;
		float oneMinusTSquare = oneMinusT * oneMinusT;
		float oneMinusTCubic = oneMinusTSquare * oneMinusT;

		float w0 = oneMinusTCubic; // -t^3 + 3t^2 - 3t + 1
		float w1 = 3 * oneMinusTSquare * segmentParams.T; // 3t^3 - 6t^2 + 3t
		float w2 = 3 * oneMinusT * tSquare; // -3t^3 + 3t^2
		float w3 = tCubic; // t^3

		Vector3 weightedP0 = w0 * P0( spline, segmentParams.Index );
		Vector3 weightedP1 = w1 * P1( spline, segmentParams.Index );
		Vector3 weightedP2 = w2 * P2( spline, segmentParams.Index );
		Vector3 weightedP3 = w3 * P3( spline, segmentParams.Index );

		return weightedP0 + weightedP1 + weightedP2 + weightedP3;
	}

	public static Vector3 GetTangent( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );
		return GetDerivative( spline, segmentParams ).Normal;
	}

	public static Vector3 GetTangent2D( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );
		return GetTangent( spline, segmentParams ).WithZ( 0 ).Normal;
	}

	public static float GetCurvature( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );

		var velocity = GetDerivative( spline, segmentParams );
		var velocitySize = velocity.Length;

		var acceleration = GetSecondDerivative( spline, segmentParams );

		var curvatureAxis = Vector3.Cross( velocity, acceleration ) / MathF.Pow( velocitySize, 3 );

		return curvatureAxis.Length;
	}

	public static Vector3 GetCurvatureAxis( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );
		var velocity = GetDerivative( spline, segmentParams );
		var acceleration = GetSecondDerivative( spline, segmentParams );

		return Vector3.Cross( velocity, acceleration ).Normal;
	}

	public static int SegmentNum( ReadOnlyCollection<Spline.Point> spline )
	{
		return spline.Count - 1;
	}

	private static void DivideSegmentRecursive(
		ReadOnlyCollection<Spline.Point> spline,
		int segmentIndex,
		SplineSampler sampler,
		float startDistance,
		float endDistance,
		float maxSquareError,
		SortedDictionary<float, Vector3> outPoints )
	{
		var dist = endDistance - startDistance;
		if ( dist <= 0 )
		{
			return;
		}

		var middleDistance = startDistance + dist * 0.5f;

		var startT = sampler.CalculateSegmentTAtDistance( segmentIndex, startDistance ).Value;
		var middleT = sampler.CalculateSegmentTAtDistance( segmentIndex, middleDistance ).Value;
		var endT = sampler.CalculateSegmentTAtDistance( segmentIndex, endDistance ).Value;

		var startPosition = GetPosition( spline, new SegmentParams { Index = segmentIndex, T = startT } );
		var middlePosition = GetPosition( spline, new SegmentParams { Index = segmentIndex, T = middleT } );
		var endPosition = GetPosition( spline, new SegmentParams { Index = segmentIndex, T = endT } );

		var startEndLine = new Line( startPosition, endPosition );
		if ( startEndLine.SqrDistance( middlePosition ) > maxSquareError )
		{
			DivideSegmentRecursive( spline, segmentIndex, sampler, startDistance, middleDistance, maxSquareError, outPoints );
			DivideSegmentRecursive( spline, segmentIndex, sampler, middleDistance, endDistance, maxSquareError, outPoints );
		}
		else
		{
			outPoints.Add( middleDistance, middlePosition );
		}
	}

	public static void ConvertSplineToPolyLine(
		ReadOnlyCollection<Spline.Point> spline,
		ref List<Vector3> outPoints,
		float maxSquareError = 0.1f )
	{
		if ( spline.Count == 0 )
		{
			return;
		}
		var sampler = new SplineSampler();
		sampler.Sample( spline );
		ConvertSplineToPolyLineWithCachedSampler( spline, ref outPoints, sampler, maxSquareError );
	}

	public static void ConvertSplineToPolyLineWithCachedSampler(
		ReadOnlyCollection<Spline.Point> spline,
		ref List<Vector3> outPoints,
		SplineSampler sampler,
		float maxSquareError = 0.1f )
	{
		outPoints.Clear();

		if ( spline.Count == 0 )
		{
			return;
		}

		var numSegments = SegmentNum( spline );
		const int averagePointsPerSegmentEstimate = 16;
		outPoints.Capacity = Math.Max( outPoints.Capacity, numSegments * averagePointsPerSegmentEstimate );

		var sortedPoints = new SortedDictionary<float, Vector3>();

		for ( int segmentIndex = 0; segmentIndex < numSegments; ++segmentIndex )
		{
			var startDist = sampler.GetSegmentStartDistance( segmentIndex );
			var stopDist = sampler.GetSegmentStartDistance( segmentIndex + 1 );

			sortedPoints.TryAdd( startDist, spline[segmentIndex].Position );
			sortedPoints.TryAdd( stopDist, spline[segmentIndex + 1].Position );

			DivideSegmentRecursive( spline, segmentIndex, sampler, startDist, stopDist, maxSquareError, sortedPoints );
		}

		foreach ( var point in sortedPoints )
		{
			outPoints.Add( point.Value );
		}
	}

	private static void CheckSegmentIndex( ReadOnlyCollection<Spline.Point> spline, int index )
	{
		if ( index < 0 || index > SegmentNum( spline ) - 1 )
		{
			throw new ArgumentOutOfRangeException( nameof( index ), "Index is out of range." );
		}
	}

	private static void CheckSegmentParams( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentIndex( spline, segmentParams.Index );
		if ( segmentParams.T < 0.0f || segmentParams.T > 1.0f )
		{
			throw new ArgumentOutOfRangeException( nameof( segmentParams.T ), "T must be in the range [0.0, 1.0]." );
		}
	}

	private static Vector3 GetDerivative( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );

		// Calculate the derivative of the cubic Bernstein polynominal
		// B'(t) = 3 * (1 - t) ^2 * (P1 - P0) + 6 * (1 - t) * t * (P2 - P1) + 3 * t^2 * (P3 - P2)
		float t2 = segmentParams.T * segmentParams.T;

		float w0 = -3 * t2 + 6 * segmentParams.T - 3;
		float w1 = 9 * t2 - 12 * segmentParams.T + 3;
		float w2 = -9 * t2 + 6 * segmentParams.T;
		float w3 = 3 * t2;

		Vector3 weightedP0 = w0 * P0( spline, segmentParams.Index );
		Vector3 weightedP1 = w1 * P1( spline, segmentParams.Index );
		Vector3 weightedP2 = w2 * P2( spline, segmentParams.Index );
		Vector3 weightedP3 = w3 * P3( spline, segmentParams.Index );

		return weightedP0 + weightedP1 + weightedP2 + weightedP3;
	}

	private static Vector3 GetSecondDerivative( ReadOnlyCollection<Spline.Point> spline,
		SegmentParams segmentParams )
	{
		CheckSegmentParams( spline, segmentParams );

		// Calculate the second derivative of the cubic Bernstein polynominal
		// B''(t) = 6 * (1 - t) * (P2 - 2 * P1 + P0) + 6 * t * (P3 - 2 * P2 + P1)
		float w0 = -6 * segmentParams.T + 6;
		float w1 = 18 * segmentParams.T - 12;
		float w2 = -18 * segmentParams.T + 6;
		float w3 = 6 * segmentParams.T;

		Vector3 weightedP0 = w0 * P0( spline, segmentParams.Index );
		Vector3 weightedP1 = w1 * P1( spline, segmentParams.Index );
		Vector3 weightedP2 = w2 * P2( spline, segmentParams.Index );
		Vector3 weightedP3 = w3 * P3( spline, segmentParams.Index );

		return weightedP0 + weightedP1 + weightedP2 + weightedP3;
	}

	private static Vector3 GetThirdDerivative( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		CheckSegmentIndex( spline, segmentIndex );

		// Calculate the third derivative of the cubic Bernstein polynominal
		// B'''(t) = - 6 * P0 + 18 * P1 - 18 * P2 + 6 * P3
		const float w0 = -6;
		const float w1 = 18;
		const float w2 = -18;
		const float w3 = 6;

		Vector3 weightedP0 = w0 * P0( spline, segmentIndex );
		Vector3 weightedP1 = w1 * P1( spline, segmentIndex );
		Vector3 weightedP2 = w2 * P2( spline, segmentIndex );
		Vector3 weightedP3 = w3 * P3( spline, segmentIndex );

		return weightedP0 + weightedP1 + weightedP2 + weightedP3;
	}

	// In 3D we can only calculate the Root for each axis separately
	// We accomplish this by projecting the spline segment onto the axis and then solving the 1D cubic equation
	private static List<float> CalculateRootsForAxis( ReadOnlyCollection<Spline.Point> spline, int segmentIndex, int axis )
	{
		CheckSegmentIndex( spline, segmentIndex );

		var p0Component = P0( spline, segmentIndex )[axis];
		var p1Component = P1( spline, segmentIndex )[axis];
		var p2Component = P2( spline, segmentIndex )[axis];
		var p3Component = P3( spline, segmentIndex )[axis];

		var a = -p0Component + 3 * (p1Component - p2Component) + p3Component;
		var b = 3 * (p0Component - 2 * p1Component + p2Component);
		var c = 3 * (-p0Component + p1Component);
		var d = p0Component;

		var roots = MathX.SolveCubic( a, b, c, d );
		roots.RemoveAll( root => root < 0.0f || root > 1.0f );

		return roots;
	}

	// In 3D we can only calculate the extrema for each axis separately
	// We accmplish this by projecting the spline segment onto the axis and then solving the 1D quadratic equation of the derivative
	private static List<float> CalculateLocalExtremaForAxis( ReadOnlyCollection<Spline.Point> spline, int segmentIndex,
		int axis )
	{
		CheckSegmentIndex( spline, segmentIndex );

		var p0Component = P0( spline, segmentIndex )[axis];
		var p1Component = P1( spline, segmentIndex )[axis];
		var p2Component = P2( spline, segmentIndex )[axis];
		var p3Component = P3( spline, segmentIndex )[axis];

		// Solve derivative for extrema
		var a = 3 * (-p0Component + 3 * (p1Component - p2Component) + p3Component);
		var b = 6 * (p0Component - 2 * p1Component + p2Component);
		var c = 3 * (-p0Component + p1Component);

		var roots = MathX.SolveQuadratic( a, b, c );
		roots.RemoveAll( root => root < 0.0f || root > 1.0f );

		return roots;
	}

	// For the inflections we project the spline segment onto the axis and then solve the 1D cubic equation of the second derivative
	public static List<float> CalculateInflections2D( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		CheckSegmentIndex( spline, segmentIndex );

		var alignmentInfo = CalculateAlignmentInfoForPoints( P0( spline, segmentIndex ), P3( spline, segmentIndex ) );
		var alignedSplineSegment = AlignSegment( spline, segmentIndex, alignmentInfo );

		var x1 = P1( alignedSplineSegment.AsReadOnly(), 0 ).x;
		var x2 = P2( alignedSplineSegment.AsReadOnly(), 0 ).x;
		var x3 = P3( alignedSplineSegment.AsReadOnly(), 0 ).x;

		var y1 = P1( alignedSplineSegment.AsReadOnly(), 0 ).y;
		var y2 = P2( alignedSplineSegment.AsReadOnly(), 0 ).y;

		var p = x2 * y1;
		var q = x3 * y1;
		var r = x1 * y2;
		var s = x3 * y2;

		var a = (-3 * p + 2 * q + 3 * r - s) * 18;
		var b = (3 * p - q - 3 * r) * 18;
		var c = (r - p) * 18;

		var roots = MathX.SolveQuadratic( a, b, c );
		roots.RemoveAll( root => root < 0.0f || root > 1.0f );

		return roots;
	}

	// Helper function used to add the extrema to the bounding box
	private static void AddExtremaToBox( ReadOnlyCollection<Spline.Point> spline, int segmentIndex, ref BBox inBox )
	{
		for ( int axis = 0; axis < 3; ++axis )
		{
			var extrema = CalculateLocalExtremaForAxis( spline, segmentIndex, axis );
			foreach ( var extremaT in extrema )
			{
				inBox = inBox.AddPoint( GetPosition( spline,
					new SegmentParams { Index = segmentIndex, T = extremaT } ) );
			}
		}
	}

	// The bounding box of a spline segment is defined by the start and end points of the segment and local extrema.
	private static BBox CalculateBoundingBoxForSegment( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		BBox result = BBox.FromPositionAndSize( GetPosition( spline, new SegmentParams { Index = segmentIndex, T = 0f } ) );

		result = result.AddPoint( GetPosition( spline, new SegmentParams { Index = segmentIndex, T = 1f } ) );
		AddExtremaToBox( spline, segmentIndex, ref result );

		return result;
	}


	public static bool IsLoop( ReadOnlyCollection<Spline.Point> spline )
	{
		return spline.Count >= 3 && spline[0].Position.AlmostEqual( spline[spline.Count - 1].Position, 0.01f );
	}

	// Calculates the Bounding box of the entire spline, by combining the bounding boxes of each segment.
	public static BBox CalculateBoundingBox( ReadOnlyCollection<Spline.Point> spline )
	{
		var inputSegmentNum = SegmentNum( spline );

		if ( inputSegmentNum == 0 )
		{
			return BBox.FromPositionAndSize( Vector3.Zero );
		}

		BBox result = CalculateBoundingBoxForSegment( spline, 0 );

		for ( int segmentIndex = 1; segmentIndex < inputSegmentNum; ++segmentIndex )
		{
			result = result.AddBBox( CalculateBoundingBoxForSegment( spline, segmentIndex ) );
		}

		return result;
	}

	private static BBox CalculateAlignedBoundingBoxForSegment( ReadOnlyCollection<Spline.Point> spline, int segmentIndex,
		SegmentAlignmentInfo alignmentInfo )
	{
		var alignedSplineSegment = AlignSegment( spline, segmentIndex, alignmentInfo );

		return CalculateBoundingBoxForSegment( alignedSplineSegment.AsReadOnly(), 0 );
	}

	private static OrientedBoundingBox ReverseAlignmentForBoundingBox( BBox alignedBoundingBox,
		SegmentAlignmentInfo alignmentInfo )
	{
		Transform boxTransform = Transform.Zero;
		boxTransform.Position =
			alignmentInfo.RotationInverse * alignedBoundingBox.Center + alignmentInfo.TranslationInverse;
		boxTransform.Rotation = alignmentInfo.RotationInverse;

		return new OrientedBoundingBox { Transform = boxTransform, Extents = alignedBoundingBox.Extents };
	}

	public static OrientedBoundingBox CalculateMinOrientedBoundingBox( ReadOnlyCollection<Spline.Point> spline )
	{
		var inputSegmentNum = SegmentNum( spline );

		if ( inputSegmentNum == 0 )
		{
			return new OrientedBoundingBox
			{
				Transform = Transform.Zero,
				Extents = new Vector3( 0, 0, 0 )
			};
		}

		OrientedBoundingBox currentMinBBox = new OrientedBoundingBox
		{
			Transform = Transform.Zero,
			Extents = new Vector3( float.MaxValue, float.MaxValue, float.MaxValue )
		};
		for ( int alignmentSegmentCandidateIndex = 0;
			 alignmentSegmentCandidateIndex < inputSegmentNum;
			 ++alignmentSegmentCandidateIndex )
		{
			var alignmentInfo = CalculateAlignmentInfoForPoints( P0( spline, alignmentSegmentCandidateIndex ),
				P3( spline, alignmentSegmentCandidateIndex ) );

			BBox alignedBoundingBox = CalculateAlignedBoundingBoxForSegment( spline, 0, alignmentInfo );
			for ( int segmentIndex = 1; segmentIndex < inputSegmentNum; ++segmentIndex )
			{
				alignedBoundingBox =
					alignedBoundingBox.AddBBox(
						CalculateAlignedBoundingBoxForSegment( spline, segmentIndex, alignmentInfo ) );
			}

			if ( alignedBoundingBox.Extents.LengthSquared < currentMinBBox.Extents.LengthSquared )
			{
				currentMinBBox = ReverseAlignmentForBoundingBox( alignedBoundingBox, alignmentInfo );
			}
		}

		return currentMinBBox;
	}

	private static double CalculateArea2DForSegment( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		// http://ich.deanmcnamee.com/graphics/2016/03/30/CurveArea.html
		var point0 = P0( spline, segmentIndex );
		var point1 = P1( spline, segmentIndex );
		var point2 = P2( spline, segmentIndex );
		var point3 = P3( spline, segmentIndex );

		return 3.0 *
			   ((point3.y - point0.y) * (point1.x + point2.x) -
				(point3.x - point0.x) * (point1.y + point2.y) +
				point1.y * (point0.x - point2.x) - point1.x * (point0.y - point2.y) +
				point3.y * (point2.x + point0.x / 3.0) - point3.x * (point2.y + point0.y / 3.0)) /
			   20.0;
	}

	public static double CalculateArea2D( ReadOnlyCollection<Spline.Point> spline )
	{
		if ( !IsLoop( spline ) )
		{
			throw new ArgumentException( nameof( spline ), "The spline must be a loop (first and last points must be almost equal)." );
		}

		var splineSegmentNum = SegmentNum( spline );
		double result = 0.0;

		for ( int segmentIndex = 0; segmentIndex < splineSegmentNum; ++segmentIndex )
		{
			result += CalculateArea2DForSegment( spline, segmentIndex );
		}

		return result;
	}

	public static Vector3 P0( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		CheckSegmentIndex( spline, segmentIndex );
		return spline[segmentIndex].Position;
	}

	public static Vector3 P1( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		CheckSegmentIndex( spline, segmentIndex );
		return spline[segmentIndex].Position + spline[segmentIndex].Out;
	}

	public static Vector3 P2( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		CheckSegmentIndex( spline, segmentIndex );
		return spline[segmentIndex + 1].Position + spline[segmentIndex + 1].In;
	}

	public static Vector3 P3( ReadOnlyCollection<Spline.Point> spline, int segmentIndex )
	{
		CheckSegmentIndex( spline, segmentIndex );
		return spline[segmentIndex + 1].Position;
	}

	public struct SplitSegmentResult
	{
		public Spline.Point Left;
		public Spline.Point Mid;
		public Spline.Point Right;
	}

	// https://www.youtube.com/watch?v=lPJo1jayLdc
	public static SplitSegmentResult SplitSegment( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams, float distanceParam, bool inferTangentMode = false )
	{
		CheckSegmentParams( spline, segmentParams );

		var point0 = P0( spline, segmentParams.Index );
		var point1 = P1( spline, segmentParams.Index );
		var point2 = P2( spline, segmentParams.Index );
		var point3 = P3( spline, segmentParams.Index );

		var leftOut = point0 + (point1 - point0) * segmentParams.T;
		var b = point1 + (point2 - point1) * segmentParams.T;
		var rightIn = point2 + (point3 - point2) * segmentParams.T;
		var midIn = leftOut + (b - leftOut) * segmentParams.T;
		var midOut = b + (rightIn - b) * segmentParams.T;
		var midPoint = midIn + (midOut - midIn) * segmentParams.T;

		var result = new SplitSegmentResult
		{
			Left = new Spline.Point
			{
				Position = point0,
				In = spline[segmentParams.Index].In,
				Out = leftOut - point0,
				Mode = inferTangentMode ? spline[segmentParams.Index].Mode : Spline.HandleMode.Split,
				Roll = spline[segmentParams.Index].Roll,
				Scale = spline[segmentParams.Index].Scale,
				Up = spline[segmentParams.Index].Up
			},
			Mid = new Spline.Point
			{
				Position = midPoint,
				In = midIn - midPoint,
				Out = midOut - midPoint,
				Mode = inferTangentMode ? InferTangentModeForSplitPoint( spline, segmentParams ) : Spline.HandleMode.Split,
				Roll = MathX.Lerp( spline[segmentParams.Index].Roll, spline[segmentParams.Index + 1].Roll, distanceParam ),
				Scale = Vector3.Lerp( spline[segmentParams.Index].Scale, spline[segmentParams.Index + 1].Scale, distanceParam ),
				Up = Vector3.Lerp( spline[segmentParams.Index].Up, spline[segmentParams.Index + 1].Up, distanceParam )

			},
			Right = new Spline.Point
			{
				Position = point3,
				In = rightIn - point3,
				Out = spline[segmentParams.Index + 1].Out,
				Mode = inferTangentMode ? spline[segmentParams.Index + 1].Mode : Spline.HandleMode.Split,
				Roll = spline[segmentParams.Index + 1].Roll,
				Scale = spline[segmentParams.Index + 1].Scale,
				Up = spline[segmentParams.Index + 1].Up
			}
		};

		return result;
	}

	private static Spline.HandleMode InferTangentModeForSplitPoint( ReadOnlyCollection<Spline.Point> spline, SegmentParams segmentParams )
	{
		// If the tangent modes are the same on both sides we just assume the new points should have the same tangent mode
		if ( spline[segmentParams.Index].Mode == spline[segmentParams.Index + 1].Mode )
		{
			return spline[segmentParams.Index].Mode;
		}

		// if one of them uses auto asume the new points should use auto
		if ( spline[segmentParams.Index].Mode == Spline.HandleMode.Auto ||
			 spline[segmentParams.Index + 1].Mode == Spline.HandleMode.Auto )
		{
			return Spline.HandleMode.Auto;
		}

		// If one of them uses linear assume the new points should use linear
		if ( spline[segmentParams.Index].Mode == Spline.HandleMode.Linear ||
			 spline[segmentParams.Index + 1].Mode == Spline.HandleMode.Linear )
		{
			return Spline.HandleMode.Linear;
		}

		// Otherwise we default to custom
		return Spline.HandleMode.Split;
	}


	// https://github.com/erich666/GraphicsGems/blob/master/gems/FitCurves.c
	public static SegmentParams FindSegmentAndTClosestToPosition(
		ReadOnlyCollection<Spline.Point> spline,
		Vector3 queryPosition )
	{
		const int pointsChecked = 3;
		const int iterationNum = 3;

		SegmentParams result = new SegmentParams();
		float closestDistanceSq = float.MaxValue;

		Span<float> initialTs = stackalloc float[pointsChecked] { 0.0f, 0.5f, 1.0f };

		for ( int segmentIndex = 0; segmentIndex < SegmentNum( spline ); ++segmentIndex )
		{
			for ( int i = 0; i < pointsChecked; ++i )
			{
				var (t, distanceSq) = NewtonRaphsonRootFind( spline, segmentIndex, queryPosition, initialTs[i], iterationNum );

				if ( distanceSq < closestDistanceSq )
				{
					closestDistanceSq = distanceSq;
					result.Index = segmentIndex;
					result.T = t;
				}
			}
		}

		return result;
	}

	private static (float, float) NewtonRaphsonRootFind(
		ReadOnlyCollection<Spline.Point> spline,
		int segmentIndex,
		Vector3 queryPosition,
		float currentCandidate,
		int iterationNum )
	{
		Vector3 delta = Vector3.Zero;
		for ( int iteration = 0; iteration < iterationNum; ++iteration )
		{
			Vector3 position = GetPosition( spline, new SegmentParams { Index = segmentIndex, T = currentCandidate } );
			Vector3 firstDerivative = GetDerivative( spline, new SegmentParams { Index = segmentIndex, T = currentCandidate } );
			Vector3 secondDerivative = GetSecondDerivative( spline, new SegmentParams { Index = segmentIndex, T = currentCandidate } );
			delta = position - queryPosition;

			float numerator = Vector3.Dot( delta, firstDerivative );
			float denominator = firstDerivative.LengthSquared + Vector3.Dot( delta, secondDerivative );

			if ( denominator == 0.0f )
				break;

			float movedCandidate = currentCandidate - numerator / denominator;
			currentCandidate = Math.Clamp( movedCandidate, 0.0f, 1.0f );
		}
		return (currentCandidate, delta.LengthSquared);
	}


	// Technically we could use a single Transform instead to store the same information.
	// But we dont need scale and the struct below makes the intent clearer
	// In addition we avoid a bunch of matrix multiplications at the cost of a bit extra memory.
	private struct SegmentAlignmentInfo
	{
		public Vector3 Translation;
		public Vector3 TranslationInverse;
		public Rotation Rotation;
		public Rotation RotationInverse;
	}

	// Calculate the translation and rotation needed to align the segment to the x-axis.
	// Specfically means that the tangent at the start of the segment is parallel to the x-axis and starts at the origin.
	private static SegmentAlignmentInfo CalculateAlignmentInfoForPoints( Vector3 axisOrigin,
		Vector3 pointOnPositiveAxis )
	{
		var alignmentTranslationInverse = axisOrigin;
		var alignmentTranslation = -alignmentTranslationInverse;

		var translatedLastPoint = pointOnPositiveAxis + alignmentTranslation;
		var alignmentRotationInverse = Rotation.From(
			MathF.Atan2( translatedLastPoint.z,
				MathF.Sqrt( translatedLastPoint.x * translatedLastPoint.x +
							translatedLastPoint.y * translatedLastPoint.y ) ) * (180.0f / MathF.PI),
			MathF.Atan2( translatedLastPoint.y, translatedLastPoint.x ) * (180.0f / MathF.PI), 0.0f );
		var alignmentRotation = alignmentRotationInverse.Inverse;

		return new SegmentAlignmentInfo
		{
			Translation = alignmentTranslation,
			TranslationInverse = alignmentTranslationInverse,
			Rotation = alignmentRotation,
			RotationInverse = alignmentRotationInverse
		};
	}

	// Align the segment to the x-axis using the translation and rotation calculated by CalculateAlignmentInfoForPoints.
	private static Spline.Point[] AlignSegment( ReadOnlyCollection<Spline.Point> spline, int segmentIndex,
		SegmentAlignmentInfo alignmentInfo )
	{
		var alignedSegment = new Spline.Point[2];
		alignedSegment[0].Position = alignmentInfo.Rotation * (spline[segmentIndex].Position + alignmentInfo.Translation);
		alignedSegment[0].In = Vector3.Zero;
		alignedSegment[0].Out = alignmentInfo.Rotation * spline[segmentIndex].Out;

		alignedSegment[1].Position =
			alignmentInfo.Rotation * (spline[segmentIndex + 1].Position + alignmentInfo.Translation);
		alignedSegment[1].In = alignmentInfo.Rotation * spline[segmentIndex + 1].Out;
		alignedSegment[1].Out = Vector3.Zero;

		return alignedSegment;
	}

	// Calculates a linear tangent for a point on the spline and returns a new spline point with the tangent.
	public static Spline.Point CalculateLinearTangentForPoint( ReadOnlyCollection<Spline.Point> spline, int pointIndex )
	{
		return spline[pointIndex] with { In = Vector3.Zero, Out = Vector3.Zero };
	}

	// Calculate a smooth tangent for a point on the spline and return new spline point with the tangent.
	public static Spline.Point CalculateSmoothTangentForPoint( ReadOnlyCollection<Spline.Point> spline, int pointIndex )
	{
		if ( spline.Count < 2 )
		{
			return spline[pointIndex];
		}

		// Unfortunatly we need to check for the loop case as we duplicate the first and last point for loops
		// so this "virtual" last point needs to be skipped.
		var isLoop = IsLoop( spline );
		int prevIndex, nextIndex;

		if ( pointIndex == 0 )
		{
			prevIndex = isLoop ? spline.Count - 2 : 0;
			nextIndex = 1;
		}
		else if ( pointIndex == spline.Count - 1 )
		{
			prevIndex = spline.Count - 2;
			nextIndex = isLoop ? 1 : spline.Count - 1;
		}
		else
		{
			prevIndex = pointIndex - 1;
			nextIndex = pointIndex + 1;
		}

		Vector3 prevPoint = spline[prevIndex].Position;
		Vector3 nextPoint = spline[nextIndex].Position;
		Vector3 currentPoint = spline[pointIndex].Position;
		Vector3 tangent = CalculateSmoothTangent( prevPoint, currentPoint, nextPoint );

		return spline[pointIndex] with { In = -tangent, Out = tangent };
	}

	// Calculate the tangent for a point on the spline.
	// Tangent is one third of the average of the two vectors from the point to the previous and next points.
	//  --------------p (previous point)
	// /
	// |              o
	// |              |
	// x (point)      |  <- tangent for point t = ((x - p) + (n - x)) * 0.5 * (1/3)
	// |              |
	// |              o
	// \
	//  --------------n (next point)
	private static Vector3 CalculateSmoothTangent( Vector3 previousPoint, Vector3 currentPoint, Vector3 nextPoint )
	{
		var prevToCurrent = currentPoint - previousPoint;
		var currentToNext = nextPoint - currentPoint;
		var tangent = (prevToCurrent + currentToNext) * 0.5f * (1f / 3f);

		return tangent;
	}

	public struct OrientedBoundingBox
	{
		public Transform Transform;
		public Vector3 Extents;
	}

	// Samples spline at regular intervals and calculates the cumulative distance along the spline.
	// Can be used to convert back and forth between distance and spline parameters (tndex, t).
	public class SplineSampler
	{
		private const int SamplesPerSegment = 16;

		private List<float> _cumulativeDistances = new();

		private int _segmentNum;

		private List<BBox> _segmentBounds = new();

		private BBox _bounds;

		public void Sample( ReadOnlyCollection<Spline.Point> spline )
		{
			_segmentNum = SegmentNum( spline );

			float cumulativeLength = 0;
			Vector3 prevPt = SplineUtils.P0( spline, 0 );
			int size = (SamplesPerSegment - 1) * SplineUtils.SegmentNum( spline ) + 2;
			CollectionsMarshal.SetCount( _cumulativeDistances, size );
			_cumulativeDistances[0] = 0;
			CollectionsMarshal.SetCount( _segmentBounds, _segmentNum );
			_bounds = BBox.FromPositionAndSize( prevPt );
			for ( int segmentIndex = 0; segmentIndex < SplineUtils.SegmentNum( spline ); segmentIndex++ )
			{
				var segmentBounds = BBox.FromPositionAndSize( prevPt );
				for ( int sampleIndex = 1; sampleIndex < SamplesPerSegment; sampleIndex++ )
				{
					Vector3 pt = SplineUtils.GetPosition( spline,
						new SegmentParams
						{
							Index = segmentIndex,
							T = sampleIndex / (float)(SamplesPerSegment - 1)
						} );
					cumulativeLength += prevPt.Distance( pt );
					_cumulativeDistances[(segmentIndex * (SamplesPerSegment - 1)) + sampleIndex] = cumulativeLength;

					segmentBounds.Mins = Vector3.Min( _segmentBounds[segmentIndex].Mins, pt );
					segmentBounds.Maxs = Vector3.Max( _segmentBounds[segmentIndex].Maxs, pt );
					prevPt = pt;
				}
				_segmentBounds[segmentIndex] = segmentBounds;
				_bounds.Mins = Vector3.Min( _bounds.Mins, _segmentBounds[segmentIndex].Mins );
				_bounds.Maxs = Vector3.Max( _bounds.Maxs, _segmentBounds[segmentIndex].Maxs );
			}
			// duplicate last point this allows (Segment = LastSegment, T = 1) as query in GetDistanceAtSplineParams
			_cumulativeDistances[_cumulativeDistances.Count - 1] = cumulativeLength;
		}

		public SegmentParams CalculateSegmentParamsAtDistance( float distance )
		{
			if ( distance < 0 )
			{
				return new SegmentParams { Index = 0, T = 0 };
			}

			int low = 0;
			int high = _segmentNum - 1;

			while ( low <= high )
			{
				int mid = (low + high) / 2;
				float startDist = GetSegmentStartDistance( mid );
				float endDist = GetSegmentStartDistance( mid + 1 );

				if ( distance < startDist )
				{
					high = mid - 1;
				}
				else if ( distance > endDist )
				{
					low = mid + 1;
				}
				else
				{
					var candidateT = CalculateSegmentTAtDistance( mid, distance );
					if ( candidateT.HasValue )
					{
						return new SegmentParams { Index = mid, T = candidateT.Value };
					}
				}
			}

			return new SegmentParams { Index = _segmentNum - 1, T = 1 };
		}

		public float? CalculateSegmentTAtDistance( int segmentIndex, float distance )
		{
			for ( int sampleIndex = 0; sampleIndex < SamplesPerSegment - 1; sampleIndex++ )
			{
				int distanceIndex = (segmentIndex * (SamplesPerSegment - 1)) + sampleIndex;
				float distPrev = _cumulativeDistances[distanceIndex];
				float distNext = _cumulativeDistances[distanceIndex + 1];
				if ( (distPrev <= distance && distance <= distNext) || distPrev.AlmostEqual( distance, 0.05f ) ||
					 distNext.AlmostEqual( distance, 0.05f ) )
				{
					float tPrev = sampleIndex / (float)(SamplesPerSegment - 1);
					float tNext = (sampleIndex + 1) / (float)(SamplesPerSegment - 1);

					float tMapped = ClampAndRemapValue( distPrev, distNext, tPrev, tNext, distance );
					return tMapped;
				}
			}

			return null;
		}

		public float GetDistanceAtSplineParams( SegmentParams segmentParams )
		{
			//Debug.Assert(segmentParams.T <= 1 && segmentParams.T >= 0);
			//Debug.Assert(segmentParams.Index < _cumulativeDistances.Count - 1);

			int sampleIndex = (int)(segmentParams.T * (SamplesPerSegment - 1));
			int distanceIndex = (segmentParams.Index * (SamplesPerSegment - 1)) + sampleIndex;

			float distPrev = _cumulativeDistances[distanceIndex];
			float distNext = _cumulativeDistances[distanceIndex + 1];

			float tPrev = sampleIndex / (float)(SamplesPerSegment - 1);
			float tNext = (sampleIndex + 1) / (float)(SamplesPerSegment - 1);

			float distanceMapped = ClampAndRemapValue( tPrev, tNext, distPrev, distNext, segmentParams.T );
			return distanceMapped;
		}

		public float GetSegmentStartDistance( int segmentIndex )
		{
			return _cumulativeDistances[segmentIndex * (SamplesPerSegment - 1)];
		}

		public float GetSegmentLength( int segmentIndex )
		{
			return GetSegmentStartDistance( segmentIndex + 1 ) - GetSegmentStartDistance( segmentIndex );
		}

		public BBox GetSegmentBounds( int segmentIndex )
		{
			return _segmentBounds[segmentIndex];
		}

		public BBox GetTotalBounds()
		{
			return _bounds;
		}

		public float TotalLength()
		{
			return _cumulativeDistances.Last();
		}

		// Clamps to the value to [inputMin, inputMax] and afterwards remaps it to the range [outputMin, outputMax].
		private float ClampAndRemapValue( float inputMin, float inputMax, float outputMin, float outputMax,
			float value )
		{
			if ( inputMin.AlmostEqual( inputMax ) )
			{
				return outputMin;
			}

			float clampedValue = Math.Clamp( value, inputMin, inputMax );
			float t = (clampedValue - inputMin) / (inputMax - inputMin);
			return outputMin + t * (outputMax - outputMin);
		}
	}
}