rj-action-library/Runtime/Math/Geometry/Curve3.cs

using Godot;
using System.Collections;
using System.Collections.Generic;

namespace Rokojori
{
	public abstract class Curve3
	{
    float _gradientSamplingRange = Mathf.Pow( 10f, -8f );
    public abstract Vector3 SampleAt( float t );
    
    public virtual void SampleMultiple( int numSamples, List<Vector3> output )
    {
      SampleMultiple( new Range( 0, 1 ), numSamples, output );
    }
    
    public virtual void SampleMultiple( Range range, int numSamples, List<Vector3> output )
    {

      var diff = range.length / ( numSamples - 1 ) ;
      
      for ( var i = 0 ; i < numSamples; i++ )
      {
        var t = range.min + i * diff;
        output.Add( SampleAt( t ) );
      }
    }

    public virtual Vector3 GradientAt( float t )
    {
      return GradientAt( t, _gradientSamplingRange );
    } 

    public virtual Vector3 GradientAt( float t, float samplingRange )
    {
      return SampleAt( t + samplingRange ) - SampleAt( t - samplingRange );
    } 

    public virtual float ComputeLength( int numSamples )
    {
      return ComputeSubLength( Range.Of_01 , numSamples );
    }    

    public virtual float ComputeSubLength( Range range, int numSamples )
    {
      var diff = range.length / ( numSamples - 1 ) ;
      var it = SampleAt( range.min );
      var length = 0f;

      for ( var i = 1 ; i < numSamples; i++ )
      {
        var t = range.min + i * diff;
        var next = SampleAt( t );
        length += ( next - it ).Length();
        it = next;
      }

      return length;
    }
    
    public virtual int ComputeNumSamplesFor( Range range,
    float minimumDistance, int minSamples = 2, 
    int numSamplesForLengthComputation = -1 )
    {
      if ( minSamples < 2 )
      { minSamples = 2; }
      
      if ( numSamplesForLengthComputation <= 0  )
      { numSamplesForLengthComputation = minSamples * 4; }

      float distance = ComputeSubLength( range, numSamplesForLengthComputation );
      float numFloatingCuts = distance / minimumDistance;
      int numSamples = Mathf.CeilToInt( numFloatingCuts );

      return Mathf.Max( minSamples, numSamples );
    }

    public Vector3 GetClosestPositionTo( Vector3 position, int numSegments = 20, int depth = 0 )
    {
      var parameter = GetClosestParameterTo( position, numSegments, depth );

      return SampleAt( parameter );
    }

    public float GetClosestParameterTo( Vector3 point, int numSegments = 20, int depth = 3 )
    {
      return GetClosestParameterTo( 0, 1, point, numSegments, depth, 0 );
    }

    public float GetDistanceTo( Vector3 point, int numSegments = 20, int depth = 3 )
    {
      var p = GetClosestPositionTo( point, numSegments, depth );
      return ( p - point ).Length();
    }

    protected float GetClosestParameterTo( float tStart, float tEnd, Vector3 position, int numSegments, int maxDepth, int currentDepth = 0 )
    {
      var tNormalizer = ( tEnd - tStart ) / (float) ( numSegments - 1 );

      var closestIndex = -1;      
      var closestDistance = float.MaxValue;
      var minT = 0f;
      var maxT = 1f;
      var startPoint = SampleAt( tStart );

      var line = new Line3();
      var lastT = tStart;

      for ( int i = 1; i < numSegments; i++ )
      {
        var t = tNormalizer * i + tStart;
        var nextCurvePoint = SampleAt( t );

        line.Set( startPoint, nextCurvePoint );
        
        var closestPoint = line.ClosestPointToPoint( position );

        var distance = ( position - closestPoint ).LengthSquared();

        if ( distance < closestDistance )
        {
          closestDistance = distance;
          closestIndex = i - 1;
          minT = lastT;
          maxT = t;
        }

        startPoint = nextCurvePoint;
        lastT = t;
      }

      if ( maxDepth != currentDepth )
      { 
        return GetClosestParameterTo( minT, maxT, position, numSegments, maxDepth, currentDepth + 1 );
      }

      var tNormalizer2 = ( maxT - minT ) / (float)(numSegments - 1 );
      
      closestDistance = float.MaxValue;
      var closestParameter = 0.5f;

      for ( int i = 0; i < numSegments; i++ )
      {
        var detailedT = i * tNormalizer2 + minT;
        var sampledPoint = SampleAt( detailedT );

        var distance = ( sampledPoint - position ).LengthSquared();

        if ( distance < closestDistance )
        {
          closestParameter = detailedT;
          closestDistance = distance;
        }
      }

      return closestParameter;
      
      
    }

        
  }
}