380 lines
9.4 KiB
C#
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 );
|
|
}
|
|
}
|
|
} |