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

380 lines
9.4 KiB
C#

using System;
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 Vector3 scale;
public float twist = 0f;
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.scale = scale;
scp.weight = weight;
scp.twist = twist;
scp.tangentBefore = tangentBefore.Clone();
scp.tangentNext = tangentNext.Clone();
return scp;
}
public SplineCurvePoint CloneForXZ( float y = 0 )
{
var cloned = Clone();
cloned.position.Y = y;
cloned.tangentBefore.position.Y = 0;
cloned.tangentNext.position.Y = 0;
return cloned;
}
public SplineCurvePoint CloneForXY( float z = 0 )
{
var cloned = Clone();
cloned.position.Z = z;
cloned.tangentBefore.position.Z = 0;
cloned.tangentNext.position.Z = 0;
return cloned;
}
public SplineCurvePoint ApplyPose( Pose p )
{
var cloned = Clone();
cloned.position = p.Apply( cloned.position );
cloned.rotation = cloned.rotation * p.rotation;
cloned.tangentBefore.position = p.Apply( cloned.tangentBefore.position );
cloned.tangentNext.position = p.Apply( cloned.tangentNext.position );
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()
{
if ( _points.Count == 0 )
{
return Vector3.Zero;
}
var min = _points[ 0 ].position;
points.ForEach( p => min = min.Min( p.position ) );
return min;
}
public Vector3 MaxPointPosition()
{
if ( _points.Count == 0 )
{
return Vector3.Zero;
}
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 ApplyPose( Pose pose )
{
var splineCurve = new SplineCurve();
splineCurve._points = Lists.Map( points, p => p.ApplyPose( pose ) );
return splineCurve;
}
public SplineCurve CloneForXZ( float y = 0 )
{
var splineCurve = new SplineCurve();
splineCurve._points = Lists.Map( points, p => p.CloneForXZ( y ) );
return splineCurve;
}
public SplineCurve CloneForXY( float z = 0 )
{
var splineCurve = new SplineCurve();
splineCurve._points = Lists.Map( points, p => p.CloneForXY( z ) );
return splineCurve;
}
public Vector3 GetPositionByPointIndex( float pointIndex )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return _points[ index ].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 Pose GetPoseByPointIndex( float pointIndex )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return Pose.Create( _points[ index ].position, _points[ index ].rotation );
}
var lower = Mathf.FloorToInt( pointIndex );
var higher = Mathf.CeilToInt( pointIndex );
var lerpAmount = pointIndex - lower;
var pLower = _points[ lower ];
var pHigher = _points[ higher ];
var position = RationalCubicBezier.Compute(
lerpAmount,
pLower.position,
pLower.tangentNext.position,
pHigher.tangentBefore.position,
pHigher.position,
pLower.weight,
pLower.tangentNext.weight,
pHigher.tangentBefore.weight,
pHigher.weight
);
var rotation = pLower.rotation.Slerp( pHigher.rotation, lerpAmount );
return Pose.Create( position, rotation );
}
public Quaternion GetRotationByPointIndex( float pointIndex )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return _points[ index ].rotation;
}
var lower = Mathf.FloorToInt( pointIndex );
var higher = Mathf.CeilToInt( pointIndex );
var lerpAmount = pointIndex - lower;
return _points[ lower ].rotation.Slerp( _points[ higher ].rotation, lerpAmount );
}
public Transform3D GetTransformAt( float normalized )
{
return GetTransformByPointIndex( NormalizedToPointIndex( normalized ) );
}
public Transform3D GetTransformByPointIndex( float pointIndex )
{
var pose = GetPoseByPointIndex( pointIndex );
var scale = GetScaleByPointIndex( pointIndex );
return Math3D.TRS( pose, scale );
}
public Vector3 GetScaleByPointIndex( float pointIndex )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return _points[ index ].scale;
}
var lower = Mathf.FloorToInt( pointIndex );
var higher = Mathf.CeilToInt( pointIndex );
var lerpAmount = pointIndex - lower;
return _points[ lower ].scale.Lerp( _points[ higher ].scale, lerpAmount );
}
public float TwistAt( float normalized )
{
return SmoothStepTwistByPointIndex( NormalizedToPointIndex( normalized ) );
}
public float GetTwistByPointIndex( float pointIndex )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return _points[ index ].twist;
}
var lower = Mathf.FloorToInt( pointIndex );
var higher = Mathf.CeilToInt( pointIndex );
var lerpAmount = pointIndex - lower;
return Mathf.Lerp( _points[ lower ].twist, _points[ higher ].twist, lerpAmount );
}
public T LerpByPointIndex<T>( float pointIndex, Func<int,T> get, Func<T,T,float,T> lerp )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return get( index );
}
var lower = Mathf.FloorToInt( pointIndex );
var higher = Mathf.CeilToInt( pointIndex );
var lerpAmount = pointIndex - lower;
return lerp( get( lower ), get( higher ), lerpAmount );
}
public T SmoothStepByPointIndex<T>( float pointIndex, Func<int,T> get, Func<T,T,float,T> lerp )
{
if ( pointIndex <= 0 || pointIndex >= ( _points.Count - 1 ) )
{
var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );
return get( index );
}
var lower = Mathf.FloorToInt( pointIndex );
var higher = Mathf.CeilToInt( pointIndex );
var lerpAmount = pointIndex - lower;
var smoothStepAmount = Mathf.SmoothStep( 0, 1, lerpAmount );
return lerp( get( lower ), get( higher ), smoothStepAmount );
}
public Vector3 SmoothStepScaleByPointIndex( float pointIndex )
{
return SmoothStepByPointIndex<Vector3>( pointIndex,
( id ) => _points[ id ].scale,
( a, b, t ) => a.Lerp( b, t )
);
}
public float SmoothStepTwistByPointIndex( float pointIndex )
{
return SmoothStepByPointIndex<float>( pointIndex,
( id ) => _points[ id ].twist,
( a, b, t ) => Mathf.Lerp( a, b, t )
);
}
public override Vector3 PositionAt( float t )
{
if ( _points.Count <= 0 )
{
return Vector3.Zero;
}
var index = NormalizedToPointIndex( t );
return GetPositionByPointIndex( index );
}
public override Quaternion RotationAt( float t )
{
if ( _points.Count <= 0 )
{
return Quaternion.Identity;
}
var index = NormalizedToPointIndex( t );
return GetRotationByPointIndex( index );
}
public float NormalizedToPointIndex( float normalized )
{
return normalized * ( _points.Count - 1 );
}
public float PointIndexToNormalized( float pointIndex )
{
return pointIndex / ( _points.Count - 1 );
}
}
}