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

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


namespace Rokojori
{

  public class SubdivisionData
  {
    public Vector3 point;
    public bool isCorner;
    public Vector3 direction;

    public SubdivisionData( Vector3 p, Vector3 d, bool corner = false )
    {
      point = p;
      direction = d;
      isCorner = corner;
    }

  }

  public class LerpCurveClosestToPointResult
  {
    public float parameter;
    public float distance;
    public Vector3 point;
  }

	public class LerpCurve3:Curve3
	{
    List<Vector3> points;
    
    public LerpCurve3( List<Vector3> list )
    {
      this.points = list;
      
      if ( list.Count == 0 )
      {
        RJLog.Log( "LERP CURVE WITH EMPTY LIST" );
      }
    }

    public override float ComputeLength( int numSamples )
    {
      var length = 0f;
      var end = points.Count - 1;

      for ( int i = 0; i < end; i++ )
      {
        var p0  = points[ i ];
        var p1  = points[ i + 1 ];

        length += p0.DistanceTo( p1 );
      }

      return length;
    }

    public int numSegments => points.Count -1;
    public float GetSegmentDistance( int i )
    {
      return ( points[ i + 1 ] - points[ i ] ).Length(); 
    }

    public float GetMinimumSegmentDistance()
    {
      var min = GetSegmentDistance( 0 );

      for ( int i = 1; i < numSegments; i++ )
      {
        min = Mathf.Min( min, GetSegmentDistance( i ) );
      }

      return min;
    }

    public LerpCurve3 CreateMinimumSpaced( float minSpace )
    {
      var lastPoint = points.Count - 1;

      var spacedPoints = new List<Vector3>();

      var line = new Line3();

      for ( int i = 0;  i < lastPoint; i++ )
      {
        line.start = points[ i ];
        line.end   = points[ i + 1 ];

        var direction = line.direction;
        var length = direction.Length();

        var numPoints = Mathf.CeilToInt( length / minSpace );

        for ( int j = 0; j < numPoints - 1; j++ )
        {
          var t = j / (float)( numPoints - 1 );
          spacedPoints.Add( line.PositionAt( t ) );
        }
      }

      spacedPoints.Add( points[ points.Count - 1 ] );

      return new LerpCurve3( spacedPoints );

    }

    public static LerpCurve3 SampledFrom( Curve3 curve, int numSamples )
    {
      var points = new List<Vector3>();

      for ( int i = 0; i < numSamples; i++ )
      {
        var t = i / ( (float) numSamples - 1 );

        var p = curve.PositionAt( t );

        points.Add( p );
      }

      return new LerpCurve3( points );
    }

    public static LerpCurve3 SampledWithResolution( Curve3 curve, float resolution, int lengthSampleResolution = 20 )
    { 
      var length = curve.ComputeLength( lengthSampleResolution );

      var numSamples = Mathf.CeilToInt( length * resolution );

      return SampledFrom( curve, numSamples );
    } 

    public static LerpCurve3 SampledEqually( Curve3 curve, float minDistance, float smallestSegmentDivision = 2, int lengthSampleResolution = 20 )
    {
      var lerpCurve = SampledWithResolution( curve, minDistance, lengthSampleResolution );

      var smallestSegment = lerpCurve.GetMinimumSegmentDistance() / smallestSegmentDivision;

      var minSpace = Mathf.Min( smallestSegment, minDistance );

      return lerpCurve.CreateMinimumSpaced( minSpace );
    }

    public float UndistortParameter( float parameter )
    {
      var completeLength = ComputeLength( 0 );

      var end = points.Count - 1;
      var normalizer = 1f / ( points.Count - 1 );

      var length = 0f;

      for ( int i = 0; i < end; i++ )
      {
        var p0  = points[ i ];
        var p1  = points[ i + 1 ];
        var t0  = i * normalizer;
        var t1  = ( i + 1 ) * normalizer;
        var distance = p0.DistanceTo( p1 );

        if ( t0 <= parameter && parameter <= t1 )
        {
          length += distance * ( parameter - t0 ); 
          return length / completeLength;
        }

        length += distance;

      }

      return 1;

    }

    public List<SubdivisionData> Subdivide( float distance, bool close )
    {
      var output = new List<SubdivisionData>();

      var num = points.Count - 1;

      if ( close )
      {
        num ++;
      }

      for ( var i = 0; i < num; i++ )
      {
        var start = points[ i ];
        var end   = points[ i == points.Count ? 0 : i + 1 ];

        var dir = ( end - start );
        var length = dir.Length();
        var samples = Mathf.CeilToInt( length / distance );

        output.Add( new SubdivisionData( start, dir, true ) );

        for ( int j = 1; j < samples; j++ )
        {
          var samplePoint = j / (float) samples;
          var point = start.Lerp( end, samplePoint );
          output.Add( new SubdivisionData( point, dir ) );
        }
        
      }
      

      if ( ! close )
      {
        var start = points[ points.Count - 2];
        var end   = points[ points.Count - 1];

        var dir = ( end - start );

        output.Add( new SubdivisionData( end, dir, true ) );
        
      }


      var dirCount = close ? output.Count : ( output.Count - 1 );

      for ( int i = 0; i < dirCount; i++ )
      {
        var next = i == output.Count ? 0 : i + 1;
        output[ i ].direction = output[ next ].point - output[ i ].point;
      }
      

      

      return output;
    }

    public override Vector3 PositionAt( float t )
    {
      if ( t < 0 )
      {
        return points[ 0 ];
      }
      
      if ( t >= 1 )
      {
        return points[ points.Count - 1 ];
      }

      if ( points.Count == 1 )
      {
        return points[ 0 ];
      }

    

      var listIndex = t * ( points.Count - 1 );
      var flooredIndex = (int) Mathf.Floor( listIndex );

      var lerpAmount = listIndex - flooredIndex;

      if ( flooredIndex < 0 ||  ( flooredIndex >= points.Count - 1 ) )
      {
        RJLog.Log( "Out of range index:",flooredIndex, " t:", t, " listIndex", listIndex, "Count:", points.Count );
      }

      var lower = points[ flooredIndex ];
      var upper = points[ flooredIndex + 1 ];

      return Math3D.LerpUnclamped( lower, upper, lerpAmount );
      
    }

    public LerpCurve3 FromRange( Curve3 curve, Range range, int numSamples )
    {
      var points = new List<Vector3>();
      curve.SampleMultiple( range, numSamples, points );
      return new LerpCurve3( points );
    }

    public LerpCurve3 From( Curve3 curve, int numSamples )
    {
      return FromRange( curve, Range.Of_01, numSamples );
    }

    public static LerpCurve3 FromPoints( Vector3 first, Vector3 second, params Vector3[] others )
    {
      var points = new List<Vector3>();
      points.Add( first );
      points.Add( second );
      points.AddRange( others );

      return new LerpCurve3( points );
    }

    Line3 line;

    public void GetClosest( Vector3 point, LerpCurveClosestToPointResult result )
    {
      var closestDistance = float.MaxValue;
      Vector3 closestPoint    = Vector3.Zero; 
      var closestParameter  = 0f;

      if ( line == null )
      {
        line = new Line3( Vector3.Zero, Vector3.Zero );
      }

      var end = points.Count - 1;
      
      var parameterNormalizer = 1f / ( points.Count - 1f );

      for ( int i = 0; i < end; i++ )
      {        
        line.start = points[ i ];
        line.end   = points[ i + 1 ];

        var currentParameter = line.ClostestParameterToPoint( point );
        var currentClosestPoint = line.GetPointAtParameter( currentParameter );
        var currentDistance = ( point - currentClosestPoint ).LengthSquared();

        if ( currentDistance < closestDistance )
        {
          closestDistance = currentDistance;
          closestPoint = currentClosestPoint;
          closestParameter = ( currentParameter + i ) * parameterNormalizer;
        }
      }

      result.distance = Mathf.Sqrt( closestDistance );
      result.point = closestPoint;
      result.parameter = closestParameter;

    }
        
  }
}