Skip to content

Non-Uniform splines #44

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
Gollorum wants to merge 6 commits into
base: main
Choose a base branch
from
Draft

Non-Uniform splines #44

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
7e4531c
Non Uniform Splines basics
Gollorum Jul 11, 2024
7f9b6a6
Fix last segment margin handle duration calc
Gollorum Jul 11, 2024
ad2acae
Update SplineSegment.cs
Gollorum Aug 7, 2024
7d2f069
some fixes + query rework
Gollorum Aug 30, 2024
41f184a
Uniform bezier foo
Gollorum Sep 2, 2024
8222fdd
Merge branch 'main' into non-uniform-splines
Gollorum Nov 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
164 changes: 108 additions & 56 deletions WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/GlobalSpline.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,103 +11,155 @@

namespace BII.WasaBii.Extra.Geometry {

[MustBeImmutable]
public interface GlobalGeometricOperations<TTime, TVel> : GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>
where TTime : unmanaged where TVel : unmanaged
{
Length GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Distance(GlobalPosition p0, GlobalPosition p1) => p0.DistanceTo(p1);

GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Sub(GlobalPosition p0, GlobalPosition p1) => p0 - p1;
GlobalPosition GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Sub(GlobalPosition p, GlobalOffset d) => p - d;
GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Sub(GlobalOffset d1, GlobalOffset d2) => d1 - d2;

GlobalPosition GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Add(GlobalPosition d1, GlobalOffset d2) => d1 + d2;
GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Add(GlobalOffset d1, GlobalOffset d2) => d1 + d2;

GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Div(GlobalOffset diff, double d) => diff / d;
GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Mul(GlobalOffset diff, double f) => diff * f;
double GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Dot(GlobalOffset a, GlobalOffset b) => a.Dot(b).AsSquareMeters();

GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.ZeroDiff => GlobalOffset.Zero;

GlobalPosition GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Lerp(GlobalPosition from, GlobalPosition to, double t) => GlobalPosition.Lerp(from, to, t);
GlobalOffset GeometricOperations<GlobalPosition, GlobalOffset, TTime, TVel>.Lerp(GlobalOffset from, GlobalOffset to, double t) => GlobalOffset.Lerp(from, to, t);
}

[Serializable]
public sealed class GlobalSpline : SpecificSplineBase<GlobalSpline, GlobalPosition, GlobalOffset> {
public sealed class GlobalSpline : SpecificSplineBase<GlobalSpline, GlobalPosition, GlobalOffset, Duration, GlobalVelocity> {

#region Factory Methods

/// <inheritdoc cref="CatmullRomSpline.FromHandlesOrThrow{TPos,TDiff}(IEnumerable{TPos},GeometricOperations{TPos,TDiff},SplineType?,bool)"/>
/// <inheritdoc cref="CatmullRomSpline.FromHandlesOrThrow{TPos,TDiff,TTime,TVel}(IEnumerable{Tuple{TPos,TTime}},GeometricOperations{TPos,TDiff,TTime,TVel},SplineType)"/>
[Pure]
public static GlobalSpline FromHandles(IEnumerable<GlobalPosition> source, SplineType? splineType = null, bool shouldLoop = false)
=> new(CatmullRomSpline.FromHandlesOrThrow(source, GeometricOperations.Instance, splineType, shouldLoop));
public static GlobalSpline FromHandles(IEnumerable<(GlobalPosition, Duration)> source, SplineType type = SplineType.Centripetal)
=> new(CatmullRomSpline.FromHandlesOrThrow(source, GeometricOperations.Instance, type));

/// <inheritdoc cref="CatmullRomSpline.FromHandles{TPos,TDiff}(TPos,System.Collections.Generic.IEnumerable{TPos},TPos,GeometricOperations{TPos,TDiff},System.Nullable{SplineType})"/>
/// <inheritdoc cref="CatmullRomSpline.FromHandlesOrThrow{TPos,TDiff,TTime,TVel}(TPos,IEnumerable{Tuple{TPos,TTime}},TPos,GeometricOperations{TPos,TDiff,TTime,TVel},SplineType)"/>
[Pure]
public static GlobalSpline FromHandles(
GlobalPosition beginMarginHandle,
IEnumerable<GlobalPosition> interpolatedHandles,
IEnumerable<(GlobalPosition, Duration)> interpolatedHandles,
GlobalPosition endMarginHandle,
SplineType? type = null
SplineType type = SplineType.Centripetal
) => new(CatmullRomSpline.FromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type));

/// <inheritdoc cref="CatmullRomSpline.FromHandlesIncludingMargin{TPos,TDiff}"/>
/// <inheritdoc cref="CatmullRomSpline.FromHandlesIncludingMargin{TPos,TDiff,TTime,TVel}"/>
[Pure]
public static GlobalSpline FromHandlesIncludingMargin(
IEnumerable<GlobalPosition> allHandlesIncludingMargin,
SplineType? type = null
) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type));
IEnumerable<Duration> segmentStartTimes,
SplineType type = SplineType.Centripetal
) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, segmentStartTimes, GeometricOperations.Instance, type));

/// <inheritdoc cref="BezierSpline.FromHandlesWithVelocities{TPos,TDiff}"/>
/// <inheritdoc cref="BezierSpline.FromHandlesWithVelocities{TPos,TDiff,TTime,TVel}"/>
[Pure]
public static GlobalSpline FromHandlesWithVelocities(
IEnumerable<(GlobalPosition position, GlobalOffset velocity)> handles, bool shouldLoop = false,
IEnumerable<(GlobalPosition position, GlobalVelocity velocity, Duration time)> handles,
bool shouldAccelerationBeContinuous = false
) => new(BezierSpline.FromHandlesWithVelocities(handles, GeometricOperations.Instance, shouldLoop, shouldAccelerationBeContinuous));
) => new(BezierSpline.FromHandlesWithVelocities(handles, GeometricOperations.Instance, shouldAccelerationBeContinuous));

/// <inheritdoc cref="BezierSpline.FromHandlesWithVelocities{TPos,TDiff}"/>
/// <inheritdoc cref="BezierSpline.FromHandlesWithVelocities{TPos,TDiff,TTime,TVel}"/>
[Pure]
public static GlobalSpline FromHandlesWithVelocitiesAndAccelerations(
IEnumerable<(GlobalPosition position, GlobalOffset velocity, GlobalOffset acceleration)> handles, bool shouldLoop = false
) => new(BezierSpline.FromHandlesWithVelocitiesAndAccelerations(handles, GeometricOperations.Instance, shouldLoop));
IEnumerable<(GlobalPosition position, GlobalVelocity velocity, GlobalVelocity acceleration, Duration time)> handles
) => new(BezierSpline.FromHandlesWithVelocitiesAndAccelerations(handles, GeometricOperations.Instance));

#endregion

[Pure]
public LocalSpline RelativeTo(TransformProvider parent) =>
new(Map(l => l.RelativeTo(parent), LocalSpline.GeometricOperations.Instance));

public GlobalSpline(Spline<GlobalPosition, GlobalOffset> wrapped) : base(wrapped) { }
protected override GlobalSpline mkNew(Spline<GlobalPosition, GlobalOffset> toWrap) => new(toWrap);
public GlobalSpline(Spline<GlobalPosition, GlobalOffset, Duration, GlobalVelocity> wrapped) : base(wrapped) { }
protected override GlobalSpline mkNew(Spline<GlobalPosition, GlobalOffset, Duration, GlobalVelocity> toWrap) => new(toWrap);

[MustBeImmutable][Serializable]
public sealed class GeometricOperations : GeometricOperations<GlobalPosition, GlobalOffset> {

public sealed class GeometricOperations : GlobalGeometricOperations<Duration, GlobalVelocity>
{
public static readonly GeometricOperations Instance = new();

private GeometricOperations() { }
public GlobalVelocity ZeroVel => GlobalVelocity.Zero;
public Duration ZeroTime => Duration.Zero;
public double Div(Duration a, Duration b) => a / b;
public GlobalOffset Mul(GlobalVelocity v, Duration t) => v * t;
public GlobalVelocity Div(GlobalOffset d, Duration t) => d / t;
public Duration Add(Duration a, Duration b) => a + b;
public Duration Sub(Duration a, Duration b) => a - b;
public Duration Mul(Duration a, double b) => a * b;
}

}

public Length Distance(GlobalPosition p0, GlobalPosition p1) => p0.DistanceTo(p1);
[Serializable]
public sealed class UniformGlobalSpline : SpecificSplineBase<UniformGlobalSpline, GlobalPosition, GlobalOffset, double, GlobalOffset> {

public GlobalOffset Sub(GlobalPosition p0, GlobalPosition p1) => p0 - p1;
public GlobalPosition Sub(GlobalPosition p, GlobalOffset d) => p - d;
#region Factory Methods

/// <inheritdoc cref="CatmullRomSpline.UniformFromHandlesOrThrow{TPos,TDiff}(IEnumerable{TPos},GeometricOperations{TPos,TDiff,double,TDiff},SplineType,bool)"/>
[Pure]
public static UniformGlobalSpline FromHandles(IEnumerable<GlobalPosition> source, SplineType type = SplineType.Centripetal, bool shouldLoop = false)
=> new(CatmullRomSpline.UniformFromHandlesOrThrow(source, GeometricOperations.Instance, type, shouldLoop));

/// <inheritdoc cref="CatmullRomSpline.UniformFromHandlesOrThrow{TPos,TDiff}(TPos,IEnumerable{TPos},TPos,GeometricOperations{TPos,TDiff,double,TDiff},SplineType)"/>
[Pure]
public static UniformGlobalSpline FromHandles(
GlobalPosition beginMarginHandle,
IEnumerable<GlobalPosition> interpolatedHandles,
GlobalPosition endMarginHandle,
SplineType type = SplineType.Centripetal
) => new(CatmullRomSpline.UniformFromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type));

public GlobalOffset Sub(GlobalOffset d1, GlobalOffset d2) => d1 - d2;
/// <inheritdoc cref="CatmullRomSpline.UniformFromHandlesIncludingMarginOrThrow{TPos,TDiff}"/>
[Pure]
public static UniformGlobalSpline FromHandlesIncludingMargin(
IEnumerable<GlobalPosition> allHandlesIncludingMargin,
SplineType type = SplineType.Centripetal
) => new(CatmullRomSpline.UniformFromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type));

public GlobalPosition Add(GlobalPosition d1, GlobalOffset d2) => d1 + d2;
/// <inheritdoc cref="BezierSpline.UniformFromHandlesWithTangents{TPos,TDiff}"/>
[Pure]
public static UniformGlobalSpline FromHandlesWithTangents(
IEnumerable<(GlobalPosition position, GlobalOffset tangent)> handles, bool shouldLoop = false,
bool shouldAccelerationBeContinuous = false
) => new(BezierSpline.UniformFromHandlesWithTangents(handles, GeometricOperations.Instance, shouldLoop, shouldAccelerationBeContinuous));

public GlobalOffset Add(GlobalOffset d1, GlobalOffset d2) => d1 + d2;
/// <inheritdoc cref="BezierSpline.UniformFromHandlesWithTangentsAndCurvature{TPos,TDiff}"/>
[Pure]
public static UniformGlobalSpline FromHandlesWithTangentsAndCurvature(
IEnumerable<(GlobalPosition position, GlobalOffset tangent, GlobalOffset curvature)> handles, bool shouldLoop = false
) => new(BezierSpline.UniformFromHandlesWithTangentsAndCurvature(handles, GeometricOperations.Instance, shouldLoop));

public GlobalOffset Div(GlobalOffset diff, double d) => diff / d;
#endregion

public GlobalOffset Mul(GlobalOffset diff, double f) => diff * f;
[Pure]
public UniformLocalSpline RelativeTo(TransformProvider parent) =>
new(Map(l => l.RelativeTo(parent), UniformLocalSpline.GeometricOperations.Instance));

public double Dot(GlobalOffset a, GlobalOffset b) => a.Dot(b).AsSquareMeters();

public GlobalOffset ZeroDiff => GlobalOffset.Zero;
public UniformGlobalSpline(Spline<GlobalPosition, GlobalOffset, double, GlobalOffset> wrapped) : base(wrapped) { }
protected override UniformGlobalSpline mkNew(Spline<GlobalPosition, GlobalOffset, double, GlobalOffset> toWrap) => new(toWrap);

public GlobalPosition Lerp(GlobalPosition from, GlobalPosition to, double t) => GlobalPosition.Lerp(from, to, t);
public GlobalOffset Lerp(GlobalOffset from, GlobalOffset to, double t) => GlobalOffset.Lerp(from, to, t);
[MustBeImmutable][Serializable]
public sealed class GeometricOperations : GlobalGeometricOperations<double, GlobalOffset>
{
public static readonly GeometricOperations Instance = new();
public GlobalOffset ZeroVel => GlobalOffset.Zero;
public double ZeroTime => 0;
public double Div(double a, double b) => a / b;
public double Add(double a, double b) => a + b;
public double Sub(double a, double b) => a - b;
public double Mul(double a, double b) => a * b;
public GlobalOffset Mul(GlobalOffset diff, double f) => diff * f;
public GlobalOffset Div(GlobalOffset diff, double f) => diff / f;
}

}

public static class GlobalSplineExtensions {

/// <inheritdoc cref="EnumerableClosestOnSplineExtensions.QueryClosestPositionOnSplinesTo{TWithSpline, TPos, TDiff}"/>
[Pure] public static Option<(TWithSpline closestSpline, ClosestOnSplineQueryResult<GlobalPosition, GlobalOffset> queryResult)> QueryClosestPositionOnSplinesTo<TWithSpline>(
this IEnumerable<TWithSpline> splines,
Func<TWithSpline, GlobalSpline> splineSelector,
GlobalPosition position,
int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples
) => splines.QueryClosestPositionOnSplinesTo<TWithSpline, GlobalPosition, GlobalOffset>(splineSelector, position, samples);

/// <inheritdoc cref="EnumerableClosestOnSplineExtensions.QueryClosestPositionOnSplinesToOrThrow{TWithSpline, TPos, TDiff}"/>
[Pure] public static (TWithSpline closestSpline, ClosestOnSplineQueryResult<GlobalPosition, GlobalOffset> queryResult) QueryClosestPositionOnSplinesToOrThrow<TWithSpline>(
this IEnumerable<TWithSpline> splines,
Func<TWithSpline, GlobalSpline> splineSelector,
GlobalPosition position,
int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples
) => splines.QueryClosestPositionOnSplinesToOrThrow<TWithSpline, GlobalPosition, GlobalOffset>(splineSelector, position, samples);

}
}
Loading