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

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

namespace Rokojori
{
  public class SplineCurveTangent
  {
    public Vector3 position;
    public float weight = 1f;

    public SplineCurveTangent Clone()
    {
      var t = new SplineCurveTangent();
      t.position = position;
      t.weight = weight;

      return t;
    }

  } 

  public class SplineCurvePoint
  {
    public Vector3 position;
    public Quaternion rotation;
    public float weight = 1f; 

    public SplineCurveTangent tangentBefore = new SplineCurveTangent();
    public SplineCurveTangent tangentNext = new SplineCurveTangent();

    public SplineCurvePoint Clone()
    {
      var scp = new SplineCurvePoint();
      scp.position = position;
      scp.rotation = rotation;
      scp.weight = weight;
      scp.tangentBefore = tangentBefore.Clone();
      scp.tangentNext = tangentNext.Clone();

      return scp;
    }

    public SplineCurvePoint CloneForXZ( float y )
    {
      var cloned = Clone();
      cloned.position.Y = y;
      cloned.tangentBefore.position.Y = 0;
      cloned.tangentNext.position.Y = 0;

      return cloned;
    }

  }

  public class SplineCurve: Curve3
  {
    List<SplineCurvePoint> _points = new List<SplineCurvePoint>();
    
    public List<SplineCurvePoint> points => _points;

    public static SplineCurve From( List<SplineCurvePoint> points )
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = points;
      return splineCurve;
    }

    public Vector3 MinPointPosition()
    {
      var min = _points[ 0 ].position;

      points.ForEach( p => min = min.Min( p.position ) );

      return min;
    }

    public Vector3 MaxPointPosition()
    {
      var max = _points[ 0 ].position;

      points.ForEach( p => max = max.Max( p.position ) );

      return max;
    }


    public SplineCurve Clone()
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = Lists.Map( points, p => p.Clone() );
      return splineCurve;
    }

    public SplineCurve CloneForXZ( float y )
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = Lists.Map( points, p => p.CloneForXZ( y ) );
      return splineCurve;
    }

    
    public Vector3 GetByPointIndex( float pointIndex )
    {
      if ( pointIndex <= 0 )
      {
        return _points[ 0 ].position;
      }

      if ( pointIndex >= ( _points.Count - 1 ) )
      {
        return _points[ _points.Count - 1 ].position;
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      return RationalCubicBezier.Compute( 
        lerpAmount,

        _points[ lower ].position, 
        _points[ lower ].tangentNext.position,
        _points[ higher ].tangentBefore.position,
        _points[ higher ].position,

        _points[ lower ].weight, 
        _points[ lower ].tangentNext.weight,
        _points[ higher ].tangentBefore.weight,
        _points[ higher ].weight
        
      );
    }

    public override Vector3 SampleAt( float t )
    {
      if ( _points.Count <= 0 )
      {
        return Vector3.Zero;
      }

      var index = NormalizedToPointIndex( t );

      return GetByPointIndex( index );
    }

    public float NormalizedToPointIndex( float normalized )
    {
      return normalized * ( _points.Count - 1 );
    }

    public float PointIndexToNormalized( float pointIndex )
    {
      return pointIndex / ( _points.Count - 1 );
    }
  }
}