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> list;
    
    public LerpCurve3( List<Vector3> list )
    {
      this.list = list;
      
      if ( list.Count == 0 )
      {
        RJLog.Log( "LERP CURVE WITH EMPTY LIST" );
      }
    }

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

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

        length += p0.DistanceTo( p1 );
      }

      return length;
    }

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

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

      var length = 0f;

      for ( int i = 0; i < end; i++ )
      {
        var p0  = list[ i ];
        var p1  = list[ 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 = list.Count - 1;

      if ( close )
      {
        num ++;
      }

      for ( var i = 0; i < num; i++ )
      {
        var start = list[ i ];
        var end   = list[ i == list.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 = list[ list.Count - 2];
        var end   = list[ list.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 SampleAt( float t )
    {
      if ( t < 0 )
      {
        return list[ 0 ];
      }
      
      if ( t >= 1 )
      {
        return list[ list.Count - 1 ];
      }

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

    

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

      var lerpAmount = listIndex - flooredIndex;

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

      var lower = list[ flooredIndex ];
      var upper = list[ 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 = list.Count - 1;
      
      var parameterNormalizer = 1f / ( list.Count - 1f );

      for ( int i = 0; i < end; i++ )
      {        
        line.start = list[ i ];
        line.end   = list[ 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;

    }
        
  }
}