82 lines
3.0 KiB
C#
82 lines
3.0 KiB
C#
using System.Linq;
|
|
using System.Runtime.CompilerServices;
|
|
|
|
namespace Content.Shared._White.Spline.CubicBezier;
|
|
|
|
public abstract class SplineCubicBezier<T> : Spline<T>
|
|
{
|
|
protected const int LookupPrecision = 100;
|
|
|
|
protected static readonly (float c0, float c1, float c2, float c3)[] PositionCoefficientLookup
|
|
= Enumerable.Range(0, LookupPrecision + 1).Select(x => CalculateCoefficientsPosition((float) x / LookupPrecision)).ToArray();
|
|
|
|
protected static readonly (float c0, float c1, float c2, float c3)[] GradientCoefficientLookup
|
|
= Enumerable.Range(0, LookupPrecision + 1).Select(x => CalculateCoefficientsTangent((float) x / LookupPrecision)).ToArray();
|
|
|
|
protected static (float c0, float c1, float c2, float c3) CalculateCoefficientsPosition(float t)
|
|
{
|
|
var tt = t * t;
|
|
var ttt = tt * t;
|
|
return (
|
|
-ttt + 3f * tt - 3f * t + 1f,
|
|
3f * ttt - 6f * tt + 3f * t,
|
|
-3f * ttt + 3f * tt,
|
|
ttt
|
|
);
|
|
}
|
|
|
|
protected static (float c0, float c1, float c2, float c3) CalculateCoefficientsTangent(float t)
|
|
{
|
|
var tt = t * t;
|
|
return (
|
|
-3f * tt + 6f * t - 3,
|
|
9f * tt - 12f * t + 3,
|
|
-9f * tt + 6f * t,
|
|
3f * tt
|
|
);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
private static int GetLookupIndex(float t)
|
|
{
|
|
return (int) (t * LookupPrecision);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override T SamplePosition(ReadOnlySpan<T> controlPoints, float u)
|
|
{
|
|
return CalculateBezier(GetCurrentControlPoints(controlPoints, (int) u), PositionCoefficientLookup[GetLookupIndex(u % 1)]);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override T SampleVelocity(ReadOnlySpan<T> controlPoints, float u)
|
|
{
|
|
return CalculateBezier(GetCurrentControlPoints(controlPoints, (int) u), GradientCoefficientLookup[GetLookupIndex(u % 1)]);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override (T Position, T Velocity) SamplePositionVelocity(ReadOnlySpan<T> controlPoints, float u)
|
|
{
|
|
var lookupIndex = GetLookupIndex(u % 1);
|
|
var currentControlPoints = GetCurrentControlPoints(controlPoints, (int) u);
|
|
return (
|
|
CalculateBezier(currentControlPoints, PositionCoefficientLookup[lookupIndex]),
|
|
CalculateBezier(currentControlPoints, GradientCoefficientLookup[lookupIndex])
|
|
);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override float GetControlGroupAmount(int controlPointAmount)
|
|
{
|
|
return (controlPointAmount - 1) / 3f;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
protected virtual (T p0, T p1, T p2, T p3) GetCurrentControlPoints(ReadOnlySpan<T> controlPoints, int u)
|
|
{
|
|
return (controlPoints[u], controlPoints[u + 1], controlPoints[u + 2], controlPoints[u + 3]);
|
|
}
|
|
|
|
protected abstract T CalculateBezier((T p0, T p1, T p2, T p3) points, (float c0, float c1, float c2, float c3) coeffs);
|
|
}
|