diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/GlobalSpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/GlobalSpline.cs index 543f36fa..f751ca81 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/GlobalSpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/GlobalSpline.cs @@ -11,44 +11,68 @@ namespace BII.WasaBii.Extra.Geometry { + [MustBeImmutable] + public interface GlobalGeometricOperations : GeometricOperations + where TTime : unmanaged where TVel : unmanaged + { + Length GeometricOperations.Distance(GlobalPosition p0, GlobalPosition p1) => p0.DistanceTo(p1); + + GlobalOffset GeometricOperations.Sub(GlobalPosition p0, GlobalPosition p1) => p0 - p1; + GlobalPosition GeometricOperations.Sub(GlobalPosition p, GlobalOffset d) => p - d; + GlobalOffset GeometricOperations.Sub(GlobalOffset d1, GlobalOffset d2) => d1 - d2; + + GlobalPosition GeometricOperations.Add(GlobalPosition d1, GlobalOffset d2) => d1 + d2; + GlobalOffset GeometricOperations.Add(GlobalOffset d1, GlobalOffset d2) => d1 + d2; + + GlobalOffset GeometricOperations.Div(GlobalOffset diff, double d) => diff / d; + GlobalOffset GeometricOperations.Mul(GlobalOffset diff, double f) => diff * f; + double GeometricOperations.Dot(GlobalOffset a, GlobalOffset b) => a.Dot(b).AsSquareMeters(); + + GlobalOffset GeometricOperations.ZeroDiff => GlobalOffset.Zero; + + GlobalPosition GeometricOperations.Lerp(GlobalPosition from, GlobalPosition to, double t) => GlobalPosition.Lerp(from, to, t); + GlobalOffset GeometricOperations.Lerp(GlobalOffset from, GlobalOffset to, double t) => GlobalOffset.Lerp(from, to, t); + } + [Serializable] - public sealed class GlobalSpline : SpecificSplineBase { - + public sealed class GlobalSpline : SpecificSplineBase { + #region Factory Methods - /// + /// [Pure] - public static GlobalSpline FromHandles(IEnumerable 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)); - /// + /// [Pure] public static GlobalSpline FromHandles( GlobalPosition beginMarginHandle, - IEnumerable interpolatedHandles, + IEnumerable<(GlobalPosition, Duration)> interpolatedHandles, GlobalPosition endMarginHandle, - SplineType? type = null + SplineType type = SplineType.Centripetal ) => new(CatmullRomSpline.FromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type)); - /// + /// [Pure] public static GlobalSpline FromHandlesIncludingMargin( IEnumerable allHandlesIncludingMargin, - SplineType? type = null - ) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type)); + IEnumerable segmentStartTimes, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, segmentStartTimes, GeometricOperations.Instance, type)); - /// + /// [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)); - /// + /// [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 @@ -56,58 +80,86 @@ public static GlobalSpline FromHandlesWithVelocitiesAndAccelerations( public LocalSpline RelativeTo(TransformProvider parent) => new(Map(l => l.RelativeTo(parent), LocalSpline.GeometricOperations.Instance)); - public GlobalSpline(Spline wrapped) : base(wrapped) { } - protected override GlobalSpline mkNew(Spline toWrap) => new(toWrap); + public GlobalSpline(Spline wrapped) : base(wrapped) { } + protected override GlobalSpline mkNew(Spline toWrap) => new(toWrap); [MustBeImmutable][Serializable] - public sealed class GeometricOperations : GeometricOperations { - + public sealed class GeometricOperations : GlobalGeometricOperations + { 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 { - public GlobalOffset Sub(GlobalPosition p0, GlobalPosition p1) => p0 - p1; - public GlobalPosition Sub(GlobalPosition p, GlobalOffset d) => p - d; +#region Factory Methods + + /// + [Pure] + public static UniformGlobalSpline FromHandles(IEnumerable source, SplineType type = SplineType.Centripetal, bool shouldLoop = false) + => new(CatmullRomSpline.UniformFromHandlesOrThrow(source, GeometricOperations.Instance, type, shouldLoop)); + + /// + [Pure] + public static UniformGlobalSpline FromHandles( + GlobalPosition beginMarginHandle, + IEnumerable 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; + /// + [Pure] + public static UniformGlobalSpline FromHandlesIncludingMargin( + IEnumerable allHandlesIncludingMargin, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.UniformFromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type)); - public GlobalPosition Add(GlobalPosition d1, GlobalOffset d2) => d1 + d2; + /// + [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; + /// + [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 wrapped) : base(wrapped) { } + protected override UniformGlobalSpline mkNew(Spline 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 + { + 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 { - - /// - [Pure] public static Option<(TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult)> QueryClosestPositionOnSplinesTo( - this IEnumerable splines, - Func splineSelector, - GlobalPosition position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => splines.QueryClosestPositionOnSplinesTo(splineSelector, position, samples); - - /// - [Pure] public static (TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult) QueryClosestPositionOnSplinesToOrThrow( - this IEnumerable splines, - Func splineSelector, - GlobalPosition position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => splines.QueryClosestPositionOnSplinesToOrThrow(splineSelector, position, samples); - - } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/LocalSpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/LocalSpline.cs index d1c2913a..5190b356 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/LocalSpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Extra/Splines/LocalSpline.cs @@ -11,44 +11,68 @@ namespace BII.WasaBii.Extra.Geometry { + [MustBeImmutable] + public interface LocalGeometricOperations : GeometricOperations + where TTime : unmanaged where TVel : unmanaged + { + Length GeometricOperations.Distance(LocalPosition p0, LocalPosition p1) => p0.DistanceTo(p1); + + LocalOffset GeometricOperations.Sub(LocalPosition p0, LocalPosition p1) => p0 - p1; + LocalPosition GeometricOperations.Sub(LocalPosition p, LocalOffset d) => p - d; + LocalOffset GeometricOperations.Sub(LocalOffset d1, LocalOffset d2) => d1 - d2; + + LocalPosition GeometricOperations.Add(LocalPosition d1, LocalOffset d2) => d1 + d2; + LocalOffset GeometricOperations.Add(LocalOffset d1, LocalOffset d2) => d1 + d2; + + LocalOffset GeometricOperations.Div(LocalOffset diff, double d) => diff / d; + LocalOffset GeometricOperations.Mul(LocalOffset diff, double f) => diff * f; + double GeometricOperations.Dot(LocalOffset a, LocalOffset b) => a.Dot(b).AsSquareMeters(); + + LocalOffset GeometricOperations.ZeroDiff => LocalOffset.Zero; + + LocalPosition GeometricOperations.Lerp(LocalPosition from, LocalPosition to, double t) => LocalPosition.Lerp(from, to, t); + LocalOffset GeometricOperations.Lerp(LocalOffset from, LocalOffset to, double t) => LocalOffset.Lerp(from, to, t); + } + [Serializable] - public sealed class LocalSpline : SpecificSplineBase { + public sealed class LocalSpline : SpecificSplineBase { #region Factory Methods - /// + /// [Pure] - public static LocalSpline FromHandles(IEnumerable source, SplineType? splineType = null, bool shouldLoop = false) - => new(CatmullRomSpline.FromHandlesOrThrow(source, GeometricOperations.Instance, splineType, shouldLoop)); + public static LocalSpline FromHandles(IEnumerable<(LocalPosition, Duration)> source, SplineType type = SplineType.Centripetal) + => new(CatmullRomSpline.FromHandlesOrThrow(source, GeometricOperations.Instance, type)); - /// + /// [Pure] public static LocalSpline FromHandles( LocalPosition beginMarginHandle, - IEnumerable interpolatedHandles, + IEnumerable<(LocalPosition, Duration)> interpolatedHandles, LocalPosition endMarginHandle, - SplineType? type = null + SplineType type = SplineType.Centripetal ) => new(CatmullRomSpline.FromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type)); - /// + /// [Pure] public static LocalSpline FromHandlesIncludingMargin( IEnumerable allHandlesIncludingMargin, - SplineType? type = null - ) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type)); + IEnumerable segmentStartTimes, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, segmentStartTimes, GeometricOperations.Instance, type)); - /// + /// [Pure] public static LocalSpline FromHandlesWithVelocities( - IEnumerable<(LocalPosition position, LocalOffset velocity)> handles, bool shouldLoop = false, + IEnumerable<(LocalPosition position, LocalVelocity velocity, Duration time)> handles, bool shouldAccelerationBeContinuous = false - ) => new(BezierSpline.FromHandlesWithVelocities(handles, GeometricOperations.Instance, shouldLoop, shouldAccelerationBeContinuous)); + ) => new(BezierSpline.FromHandlesWithVelocities(handles, GeometricOperations.Instance, shouldAccelerationBeContinuous)); - /// + /// [Pure] public static LocalSpline FromHandlesWithVelocitiesAndAccelerations( - IEnumerable<(LocalPosition position, LocalOffset velocity, LocalOffset acceleration)> handles, bool shouldLoop = false - ) => new(BezierSpline.FromHandlesWithVelocitiesAndAccelerations(handles, GeometricOperations.Instance, shouldLoop)); + IEnumerable<(LocalPosition position, LocalVelocity velocity, LocalVelocity acceleration, Duration time)> handles + ) => new(BezierSpline.FromHandlesWithVelocitiesAndAccelerations(handles, GeometricOperations.Instance)); #endregion @@ -56,58 +80,86 @@ public static LocalSpline FromHandlesWithVelocitiesAndAccelerations( public GlobalSpline ToGlobalWith(TransformProvider parent) => new(Map(l => l.ToGlobalWith(parent), GlobalSpline.GeometricOperations.Instance)); - public LocalSpline(Spline wrapped) : base(wrapped) { } - protected override LocalSpline mkNew(Spline toWrap) => new(toWrap); + public LocalSpline(Spline wrapped) : base(wrapped) { } + protected override LocalSpline mkNew(Spline toWrap) => new(toWrap); [MustBeImmutable][Serializable] - public sealed class GeometricOperations : GeometricOperations { - + public sealed class GeometricOperations : LocalGeometricOperations + { public static readonly GeometricOperations Instance = new(); - - private GeometricOperations() { } + public LocalVelocity ZeroVel => LocalVelocity.Zero; + public Duration ZeroTime => Duration.Zero; + public double Div(Duration a, Duration b) => a / b; + public LocalOffset Mul(LocalVelocity v, Duration t) => v * t; + public LocalVelocity Div(LocalOffset 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(LocalPosition p0, LocalPosition p1) => p0.DistanceTo(p1); + [Serializable] + public sealed class UniformLocalSpline : SpecificSplineBase { - public LocalOffset Sub(LocalPosition p0, LocalPosition p1) => p0 - p1; - public LocalPosition Sub(LocalPosition p, LocalOffset d) => p - d; +#region Factory Methods + + /// + [Pure] + public static UniformLocalSpline FromHandles(IEnumerable source, SplineType type = SplineType.Centripetal, bool shouldLoop = false) + => new(CatmullRomSpline.UniformFromHandlesOrThrow(source, GeometricOperations.Instance, type, shouldLoop)); + + /// + [Pure] + public static UniformLocalSpline FromHandles( + LocalPosition beginMarginHandle, + IEnumerable interpolatedHandles, + LocalPosition endMarginHandle, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.UniformFromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type)); - public LocalOffset Sub(LocalOffset d1, LocalOffset d2) => d1 - d2; + /// + [Pure] + public static UniformLocalSpline FromHandlesIncludingMargin( + IEnumerable allHandlesIncludingMargin, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.UniformFromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type)); - public LocalPosition Add(LocalPosition d1, LocalOffset d2) => d1 + d2; + /// + [Pure] + public static UniformLocalSpline FromHandlesWithTangents( + IEnumerable<(LocalPosition position, LocalOffset tangents)> handles, bool shouldLoop = false, + bool shouldAccelerationBeContinuous = false + ) => new(BezierSpline.UniformFromHandlesWithTangents(handles, GeometricOperations.Instance, shouldLoop, shouldAccelerationBeContinuous)); - public LocalOffset Add(LocalOffset d1, LocalOffset d2) => d1 + d2; + /// + [Pure] + public static UniformLocalSpline FromHandlesWithTangentsAndCurvature( + IEnumerable<(LocalPosition position, LocalOffset tangent, LocalOffset curvature)> handles, bool shouldLoop = false + ) => new(BezierSpline.UniformFromHandlesWithTangentsAndCurvature(handles, GeometricOperations.Instance, shouldLoop)); - public LocalOffset Div(LocalOffset diff, double d) => diff / d; +#endregion - public LocalOffset Mul(LocalOffset diff, double f) => diff * f; + [Pure] + public UniformGlobalSpline ToGlobalWith(TransformProvider parent) => + new(Map(l => l.ToGlobalWith(parent), UniformGlobalSpline.GeometricOperations.Instance)); - public double Dot(LocalOffset a, LocalOffset b) => a.Dot(b).AsSquareMeters(); - - public LocalOffset ZeroDiff => LocalOffset.Zero; + public UniformLocalSpline(Spline wrapped) : base(wrapped) { } + protected override UniformLocalSpline mkNew(Spline toWrap) => new(toWrap); - public LocalPosition Lerp(LocalPosition from, LocalPosition to, double t) => LocalPosition.Lerp(from, to, t); - public LocalOffset Lerp(LocalOffset from, LocalOffset to, double t) => LocalOffset.Lerp(from, to, t); + [MustBeImmutable][Serializable] + public sealed class GeometricOperations : LocalGeometricOperations + { + public static readonly GeometricOperations Instance = new(); + public LocalOffset ZeroVel => LocalOffset.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 LocalOffset Mul(LocalOffset diff, double f) => diff * f; + public LocalOffset Div(LocalOffset diff, double f) => diff / f; } } - - public static class LocalSplineExtensions { - - /// - [Pure] public static Option<(TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult)> QueryClosestPositionOnSplinesTo( - this IEnumerable splines, - Func splineSelector, - LocalPosition position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => splines.QueryClosestPositionOnSplinesTo(splineSelector, position, samples); - - /// - [Pure] public static (TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult) QueryClosestPositionOnSplinesToOrThrow( - this IEnumerable splines, - Func splineSelector, - LocalPosition position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => splines.QueryClosestPositionOnSplinesToOrThrow(splineSelector, position, samples); - - } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Geometry/dlls/netstandard2.0/WasaBii.Geometry.Shared.dll b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Geometry/dlls/netstandard2.0/WasaBii.Geometry.Shared.dll index cb05c16e..4bb4531a 100644 Binary files a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Geometry/dlls/netstandard2.0/WasaBii.Geometry.Shared.dll and b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Geometry/dlls/netstandard2.0/WasaBii.Geometry.Shared.dll differ diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSegment.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSegment.cs index 556ac7c6..d3790fdc 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSegment.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSegment.cs @@ -9,75 +9,124 @@ namespace BII.WasaBii.Splines.Bezier { /// - /// Factory methods for constructing a that compute the handles + /// Factory methods for constructing a that compute the handles /// to fit certain criteria regarding velocity or acceleration at the curve's start and end. /// public static class BezierSegment { + /// + /// A curve starting at with tangent + /// and ending at . + /// + [Pure] + public static BezierSegment QuadraticWithTangent( + TPos start, TDiff startTangent, TPos end, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => new( + duration, + start, + ops.Add(start, ops.Div(startTangent, 2)), + end + ); /// /// A curve starting at with velocity /// and ending at . /// [Pure] - public static BezierSegment Quadratic( - TPos start, TDiff startVelocity, TPos end, - GeometricOperations ops - ) where TPos : unmanaged where TDiff : unmanaged - => new( - start, - ops.Add(start, ops.Div(startVelocity, 2)), - end - ); + public static BezierSegment QuadraticWithVelocity( + TPos start, TVel startVelocity, TPos end, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + QuadraticWithTangent(start, ops.Mul(startVelocity, duration), end, duration, ops); + /// + /// A curve starting at with tangent + /// and ending at with tangent . + /// + [Pure] + public static BezierSegment CubicWithTangents( + TPos start, TDiff startTangent, TPos end, TDiff endTangent, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => new( + duration, + start, + ops.Add(start, ops.Div(startTangent, 3)), + ops.Sub(end, ops.Div(endTangent, 3)), + end + ); /// /// A curve starting at with velocity /// and ending at with velocity . /// [Pure] - public static BezierSegment Cubic( - TPos start, TDiff startVelocity, TPos end, TDiff endVelocity, - GeometricOperations ops - ) where TPos : unmanaged where TDiff : unmanaged - => new( - start, - ops.Add(start, ops.Div(startVelocity, 3)), - ops.Sub(end, ops.Div(endVelocity, 3)), - end - ); + public static BezierSegment CubicWithVelocity( + TPos start, TVel startVelocity, TPos end, TVel endVelocity, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + CubicWithTangents(start, ops.Mul(startVelocity, duration), end, ops.Mul(endVelocity, duration), duration, ops); + /// + /// A curve starting at with tangent and curvature + /// and ending at with tangent . + /// + [Pure] + public static BezierSegment QuarticWithTangents( + TPos start, TDiff startTangent, TDiff startCurvature, TPos end, TDiff endTangent, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => new( + duration, + start, + ops.Add(start, ops.Div(startTangent, 4)), + ops.Add(start, ops.Div(ops.Add(startCurvature, ops.Mul(startTangent, 6)), 12)), + ops.Sub(end, ops.Div(endTangent, 4)), + end + ); /// /// A curve starting at with velocity and acceleration /// and ending at with velocity . /// [Pure] - public static BezierSegment Quartic( - TPos start, TDiff startVelocity, TDiff startAcceleration, TPos end, TDiff endVelocity, - GeometricOperations ops - ) where TPos : unmanaged where TDiff : unmanaged - => new( - start, - ops.Add(start, ops.Div(startVelocity, 4)), - ops.Add(start, ops.Div(ops.Add(startAcceleration, ops.Mul(startVelocity, 6)), 12)), - ops.Sub(end, ops.Div(endVelocity, 4)), - end - ); + public static BezierSegment QuarticWithVelocities( + TPos start, TVel startVelocity, TVel startAcceleration, TPos end, TVel endVelocity, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + QuarticWithTangents(start, ops.Mul(startVelocity, duration), ops.Mul(startAcceleration, duration), end, ops.Mul(endVelocity, duration), duration, ops); /// - /// A curve starting at with velocity and acceleration - /// and ending at with velocity and acceleration . + /// A curve starting at with tangent and curvature + /// and ending at with tangent and curvature . /// [Pure] - public static BezierSegment Quintic( - TPos start, TDiff startVelocity, TDiff startAcceleration, TPos end, TDiff endVelocity, TDiff endAcceleration, - GeometricOperations ops - ) where TPos : unmanaged where TDiff : unmanaged => new( + public static BezierSegment QuinticWithTangents( + TPos start, TDiff startTangent, TDiff startCurvature, TPos end, TDiff endTangent, TDiff endCurvature, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => new( + duration, start, - ops.Add(start, ops.Div(startVelocity, 5)), - ops.Add(start, ops.Div(ops.Add(startAcceleration, ops.Mul(startVelocity, 8)), 20)), - ops.Add(end, ops.Div(ops.Sub(endAcceleration, ops.Mul(endVelocity, 8)), 20)), - ops.Sub(end, ops.Div(endVelocity, 5)), + ops.Add(start, ops.Div(startTangent, 5)), + ops.Add(start, ops.Div(ops.Add(startCurvature, ops.Mul(startTangent, 8)), 20)), + ops.Add(end, ops.Div(ops.Sub(endCurvature, ops.Mul(endTangent, 8)), 20)), + ops.Sub(end, ops.Div(endTangent, 5)), end ); + /// + /// A curve starting at with velocity and acceleration + /// and ending at with velocity and acceleration . + /// + [Pure] + public static BezierSegment QuinticWithVelocities( + TPos start, TVel startVelocity, TVel startAcceleration, TPos end, TVel endVelocity, TVel endAcceleration, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + QuinticWithTangents(start, ops.Mul(startVelocity, duration), ops.Mul(startAcceleration, duration), end, ops.Mul(endVelocity, duration), ops.Mul(endAcceleration, duration), duration, ops); } @@ -88,12 +137,15 @@ GeometricOperations ops /// with just 2 handles. The curve will usually go in the direction of the handles without ever touching them. /// [Serializable] - public readonly struct BezierSegment where TPos : unmanaged where TDiff : unmanaged { + public readonly struct BezierSegment where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged + { public readonly TPos Start; public readonly ImmutableArray Handles; public readonly TPos End; + public readonly TTime Duration; + public int Degree => Handles.Length + 1; /// @@ -104,42 +156,29 @@ GeometricOperations ops /// private const int maxDegree = 12; - public BezierSegment(TPos start, ImmutableArray handles, TPos end) { + public BezierSegment(TTime duration, TPos start, ImmutableArray handles, TPos end) { Start = start; Handles = handles; End = end; + Duration = duration; if (Degree > maxDegree) throw new ArgumentException($"A single bezier curve may only have at most {maxDegree - 1} handles."); } - public BezierSegment(TPos p0, TPos p1, params TPos[] otherPos) : this( + public BezierSegment(TTime duration, TPos p0, TPos p1, params TPos[] otherPos) : this( + duration, p0, p1.PrependTo(otherPos[..^1]).ToImmutableArray(), otherPos[^1] ) { } - public TDiff StartVelocity(GeometricOperations ops) => Degree > 1 - ? ops.Mul(ops.Sub(this[1], this[0]), Degree) - : ops.Sub(End, Start); - public TDiff StartAcceleration(GeometricOperations ops) => Degree > 1 - ? ops.Mul(ops.Add(ops.Sub(this[0], this[1]), ops.Sub(this[2], this[1])), Degree * (Degree - 1)) - : ops.ZeroDiff; - - public TDiff EndVelocity(GeometricOperations ops) => Degree > 1 - ? ops.Mul(ops.Sub(this[^2], this[^1]), Degree) - : ops.Sub(End, Start); - public TDiff EndAcceleration(GeometricOperations ops) => Degree > 1 - ? ops.Mul(ops.Add(ops.Sub(this[^1], this[^2]), ops.Sub(this[^3], this[^2])), Degree * (Degree - 1)) - : ops.ZeroDiff; - - public TPos this[Index i] => i.Value == 0 ? i.IsFromEnd ? End : Start : i.Value == Degree ? i.IsFromEnd ? Start : End : Handles[new Index(i.Value - 1, i.IsFromEnd)]; - [Pure] internal Polynomial ToPolynomial(GeometricOperations ops) { + [Pure] internal Polynomial ToPolynomial(GeometricOperations ops) { var p0 = Start; var n = Handles.Length + 1; var p = new TDiff[n]; @@ -164,23 +203,24 @@ [Pure] internal Polynomial ToPolynomial(GeometricOperations(ops, p0, p); + return new Polynomial(ops, Duration, p0, p); } - [Pure] public SplineSegment ToSplineSegment(GeometricOperations ops, Lazy? cachedLength = null) + [Pure] public SplineSegment ToSplineSegment(GeometricOperations ops, Lazy? cachedLength = null) => new(ToPolynomial(ops), cachedLength); [Pure] - public BezierSegment Map(Func positionMapping) - where TPosNew : unmanaged where TDiffNew : unmanaged => new( + public BezierSegment Map(Func positionMapping) + where TPosNew : unmanaged where TDiffNew : unmanaged where TVelNew : unmanaged => new( + Duration, positionMapping(Start), Handles.Select(positionMapping).ToImmutableArray(), positionMapping(End) ); - [Pure] public BezierSegment Map(Func positionMapping) => Map(positionMapping); + [Pure] public BezierSegment Map(Func positionMapping) => Map(positionMapping); - public BezierSegment Reversed => new(End, Handles.ReverseList().ToImmutableArray(), Start); + public BezierSegment Reversed => new(Duration, End, Handles.ReverseList().ToImmutableArray(), Start); } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSpline.cs index f08bb88f..cebab2bb 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSpline.cs @@ -29,36 +29,47 @@ namespace BII.WasaBii.Splines.Bezier { /// it look less smooth. /// [Serializable] - public sealed class BezierSpline : Spline.Copyable where TPos : unmanaged where TDiff : unmanaged { + public sealed class BezierSpline : Spline.Copyable + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged + { internal sealed record Cache( - ImmutableArray> SplineSegments, - ImmutableArray SegmentOffsetsFromBegin + ImmutableArray> SplineSegments, + Length Length, + TTime TotalDuration, + ImmutableArray SpatialSegmentOffsets, + ImmutableArray TemporalSegmentOffsets ); - public readonly ImmutableArray> Segments; + public readonly ImmutableArray> Segments; [NonSerialized] private readonly Lazy cache; public int SegmentCount => Segments.Length; - IEnumerable> Spline.Segments => cache.Value.SplineSegments; + public Length Length => cache.Value.Length; + public TTime TotalDuration => cache.Value.TotalDuration; - public SplineSegment this[SplineSegmentIndex index] => cache.Value.SplineSegments[index]; - public SplineSample this[SplineLocation location] => this[this.NormalizeOrThrow(location)]; - public SplineSample this[NormalizedSplineLocation location] => - SplineSample.From(this, location).GetOrThrow(() => + IEnumerable> Spline.Segments => cache.Value.SplineSegments; + + public SplineSample this[TTime t] => + SplineSample.From(this, t); + public SplineSegment this[SplineSegmentIndex index] => cache.Value.SplineSegments[index]; + public SplineSample this[SplineLocation location] => this[this.NormalizeOrThrow(location)]; + public SplineSample this[NormalizedSplineLocation location] => + SplineSample.From(this, location).GetOrThrow(() => new ArgumentOutOfRangeException( nameof(location), location, $"Must be between 0 and {SegmentCount}" )); - public ImmutableArray SegmentOffsetsFromBegin => cache.Value.SegmentOffsetsFromBegin; + public ImmutableArray SpatialSegmentOffsets => cache.Value.SpatialSegmentOffsets; + public ImmutableArray TemporalSegmentOffsets => cache.Value.TemporalSegmentOffsets; - public GeometricOperations Ops { get; } + public GeometricOperations Ops { get; } public BezierSpline( - IEnumerable> segments, - GeometricOperations ops + IEnumerable> segments, + GeometricOperations ops ) { // Note CR: Serialization might pass only `default` parameters, so we support this case here if (segments != default) { @@ -69,38 +80,48 @@ GeometricOperations ops "Tried to construct a discontinuous spline. Each segment must " + "start at the exact position where the previous one ended." ); - } else Segments = ImmutableArray>.Empty; + } else Segments = ImmutableArray>.Empty; Ops = ops; cache = new Lazy(initCache); } - [Pure] public Spline Map( - Func positionMapping, GeometricOperations newOps - ) where TPosNew : unmanaged where TDiffNew : unmanaged - => new BezierSpline(Segments.Select(s => s.Map(positionMapping)), newOps); + [Pure] public Spline Map( + Func positionMapping, GeometricOperations newOps + ) where TPosNew : unmanaged where TDiffNew : unmanaged where TVelNew : unmanaged => + new BezierSpline(Segments.Select(s => s.Map(positionMapping)), newOps); - [Pure] public Spline Reversed => new BezierSpline(Segments.Reverse().Select(s => s.Reversed), Ops); + [Pure] public Spline Reversed => new BezierSpline(Segments.Reverse().Select(s => s.Reversed), Ops); - [Pure] public Spline CopyWithOffset(Func tangentToOffset) => + [Pure] public Spline CopyWithOffset(Func tangentToOffset) => BezierSplineCopyUtils.CopyWithOffset(this, tangentToOffset); - [Pure] public Spline CopyWithStaticOffset(TDiff offset) => + [Pure] public Spline CopyWithStaticOffset(TDiff offset) => BezierSplineCopyUtils.CopyWithStaticOffset(this, offset); - [Pure] public Spline CopyWithDifferentHandleDistance(Length desiredHandleDistance) => + [Pure] public Spline CopyWithDifferentHandleDistance(Length desiredHandleDistance) => BezierSplineCopyUtils.CopyWithDifferentHandleDistance(this, desiredHandleDistance); private Cache initCache() { var segments = ImmutableArray.CreateRange(Segments, (s, ops) => s.ToSplineSegment(ops), Ops); - var segmentOffsets = ImmutableArray.CreateBuilder(initialCapacity: Segments.Length); + var spatialOffsets = ImmutableArray.CreateBuilder(initialCapacity: Segments.Length); + var temporalOffsets = ImmutableArray.CreateBuilder(initialCapacity: Segments.Length); + var lastOffset = Length.Zero; - segmentOffsets.Add(Length.Zero); - for (var i = 1; i < segments.Length; i++) - segmentOffsets.Add(lastOffset += segments[i - 1].Length); - return new(segments, segmentOffsets.MoveToImmutable()); - } + var lastTime = Ops.ZeroTime; + foreach (var segment in segments) { + if (segment.Duration.CompareTo(Ops.ZeroTime) == -1) + throw new Exception( + $"Tried to construct a spline with a segment of negative duration: {segment.Duration}" + ); + spatialOffsets.Add(lastOffset); + temporalOffsets.Add(lastTime); + lastOffset += segment.Length; + lastTime = Ops.Add(lastTime, segment.Duration); + } + return new(segments, lastOffset, lastTime, spatialOffsets.MoveToImmutable(), temporalOffsets.MoveToImmutable()); + } } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineCopyUtils.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineCopyUtils.cs index 011c08d7..fe5fff25 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineCopyUtils.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineCopyUtils.cs @@ -8,22 +8,24 @@ public static class BezierSplineCopyUtils { /// /// Creates a new spline with a similar trajectory as , but with all handle - /// positions being moved by a certain offset which depends on the spline's tangent at these points. + /// positions being moved by a certain offset which depends on the spline's velocity at these points. /// - public static BezierSpline CopyWithOffset( - BezierSpline original, Func tangentToOffset - ) where TPos : unmanaged where TDiff : unmanaged => new( + public static BezierSpline CopyWithOffset( + BezierSpline original, Func tangentToOffset + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => new( original.Segments.Select(s => { var ops = original.Ops; - var startVelocity = s.StartVelocity(ops); - var endVelocity = s.EndVelocity(ops); + var polynomial = s.ToPolynomial(ops); + var startVelocity = polynomial.EvaluateDerivative(ops.ZeroTime); + var endVelocity = polynomial.EvaluateDerivative(s.Duration); var startOffset = tangentToOffset(startVelocity); var endOffset = tangentToOffset(endVelocity); var newStart = original.Ops.Add(s.Start, startOffset); var newEnd = original.Ops.Add(s.End, endOffset); - return s.Degree == 2 // Quadratic segment might lose velocity continuity if we don't make it cubic - ? BezierSegment.Cubic(newStart, startVelocity, newEnd, endVelocity, ops) - : new BezierSegment( + return s.Degree == 2 // Quadratic segment might lose tangent continuity if we don't make it cubic + ? BezierSegment.CubicWithVelocity(newStart, startVelocity, newEnd, endVelocity, s.Duration, ops) + : new BezierSegment( + s.Duration, newStart, s.Handles.Select((h, i) => ops.Add(h, ops.Lerp(startOffset, endOffset, i / (s.Handles.Length - 1.0)))).ToImmutableArray(), newEnd @@ -38,24 +40,27 @@ BezierSpline original, Func tangentToOffset /// being moved along a certain , /// independent of the spline's tangent at these points. /// - public static BezierSpline CopyWithStaticOffset( - BezierSpline original, TDiff offset - ) where TPos : unmanaged where TDiff : unmanaged { + public static BezierSpline CopyWithStaticOffset( + BezierSpline original, TDiff offset + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { TPos computePosition(TPos node) => original.Ops.Add(node, offset); - return new BezierSpline( + return new BezierSpline( original.Segments.Select(s => s.Map(computePosition)), original.Ops ); } /// - /// Creates a new spline with a similar trajectory as - /// , but different spacing - /// between the handles. + /// Creates a new spline with a similar trajectory as , but + /// with a uniform spacing of between the handles. /// - public static BezierSpline CopyWithDifferentHandleDistance(BezierSpline original, Length desiredHandleDistance) - where TPos : unmanaged where TDiff : unmanaged => BezierSpline.FromHandlesWithVelocities( - original.SampleSplineEvery(desiredHandleDistance).Select(sample => sample.PositionAndTangent), + /// The velocities at the new handles are preserved, the accelerations are not. + public static BezierSpline CopyWithDifferentHandleDistance( + BezierSpline original, + Length desiredHandleDistance + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => BezierSpline.FromHandlesWithVelocities( + original.SampleSplineEvery(desiredHandleDistance) + .Select(sample => (sample.Position, sample.Velocity, sample.GlobalT)), original.Ops ); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineFactory.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineFactory.cs index 42bb04b9..0d9a7d6c 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineFactory.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Bezier/BezierSplineFactory.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Collections.Immutable; using System.Diagnostics.Contracts; using System.Linq; using BII.WasaBii.Core; @@ -9,107 +10,197 @@ namespace BII.WasaBii.Splines.Bezier { /// /// Contains generic factory methods for building bezier splines. - /// For explicitly typed variants with - /// included, use `UnitySpline`, `GlobalSpline` or `LocalSpline` in the Unity assembly. + /// For explicitly typed variants with + /// included, use `UnitySpline`, `GlobalSpline` or `LocalSpline` in the Extra assembly. /// public static class BezierSpline { - + + /// + /// Constructs a spline from quadratic segments with uniform durations. The spline has a duration of 1. + /// + /// The positions the spline should visit. Every other handle will only be used for + /// influencing the segments' trajectory, it will likely not be traversed. + /// The geometric operations necessary for calculation. + /// There is no non-uniform variant to this factory since it is not possible to guarantee + /// continuous velocities across segment borders. [Pure] - public static BezierSpline FromQuadraticHandles( + public static BezierSpline UniformFromQuadraticHandles( IEnumerable handles, - GeometricOperations ops + GeometricOperations ops ) where TPos : unmanaged where TDiff : unmanaged { - using var enumerator = handles.GetEnumerator(); - var segments = new List>(); - if (!enumerator.MoveNext()) throw new ArgumentException("No handles passed. A bezier spline from quadratic segments needs at least 3 handles"); - var a = enumerator.Current; - Exception incorrectHandleCountException(int offset) => new ArgumentException( - $"Incorrect number of handles passed. A bezier spline from n quadratic segments has 2 * n + 1 handles (provided: {1 + 2 * segments.Count + offset})" - ); - while (enumerator.MoveNext()) { - var b = enumerator.Current; - if (!enumerator.MoveNext()) throw incorrectHandleCountException(1); - var c = enumerator.Current; - segments.Add(new BezierSegment(a, b, c)); - a = c; + var handleList = handles.AsReadOnlyList(); + if (handleList.IsEmpty()) + throw new ArgumentException("No handles passed. A bezier spline from quadratic segments needs at least 3 handles"); + else if (handleList.Count == 1) + throw new ArgumentException("Only one handle passed. A bezier spline from quadratic segments needs at least 3 handles"); + else if ((handleList.Count - 1) % 2 != 0) + throw new ArgumentException( + $"Incorrect number of handles passed. A bezier spline from n quadratic segments has 2 * n + 1 handles (provided: {handleList.Count})" + ); + var segments = ImmutableArray.CreateBuilder>(handleList.Count / 2); + var durationPerSegment = 1.0 / segments.Capacity; + for (var i = 0; i < segments.Capacity; i ++) { + var j = i << 1; + segments.Add(new BezierSegment( + durationPerSegment, + handleList[j], + handleList[j + 1], + handleList[j + 2] + )); } - if(segments.IsEmpty()) throw new ArgumentException("Only one handle passed. A bezier spline from quadratic segments needs at least 3 handles"); - - return new BezierSpline(segments, ops); + return new BezierSpline(segments.MoveToImmutable(), ops); } + /// + /// Constructs a spline from cubic segments with uniform durations. The spline has a duration of 1. + /// + /// The positions the spline should visit. Every two other handles will only be used for + /// influencing the segments' trajectory, they will likely not be traversed. + /// The geometric operations necessary for calculation. + /// There is no non-uniform variant to this factory since it is not possible to guarantee + /// continuous velocities across segment borders. [Pure] - public static BezierSpline FromCubicHandles( + public static BezierSpline UniformFromCubicHandles( IEnumerable handles, - GeometricOperations ops + GeometricOperations ops ) where TPos : unmanaged where TDiff : unmanaged { - using var enumerator = handles.GetEnumerator(); - var segments = new List>(); - if (!enumerator.MoveNext()) throw new ArgumentException("No handles passed. A bezier spline from cubic segments needs at least 4 handles"); - var a = enumerator.Current; - Exception incorrectHandleCountException(int offset) => new ArgumentException( - $"Incorrect number of handles passed. A bezier spline from n cubic segments has 3 * n + 1 handles (provided: {1 + 3 * segments.Count + offset})" - ); - while (enumerator.MoveNext()) { - var b = enumerator.Current; - if (!enumerator.MoveNext()) throw incorrectHandleCountException(1); - var c = enumerator.Current; - if (!enumerator.MoveNext()) throw incorrectHandleCountException(2); - var d = enumerator.Current; - segments.Add(new BezierSegment(a, b, c, d)); - a = d; + var handleList = handles.AsReadOnlyList(); + if (handleList.IsEmpty()) + throw new ArgumentException("No handles passed. A bezier spline from cubic segments needs at least 4 handles"); + else if (handleList.Count == 1) + throw new ArgumentException("Only one handle passed. A bezier spline from cubic segments needs at least 4 handles"); + else if ((handleList.Count - 1) % 3 != 0) + throw new ArgumentException( + $"Incorrect number of handles passed. A bezier spline from n cubic segments has 3 * n + 1 handles (provided: {handleList.Count})" + ); + var segments = ImmutableArray.CreateBuilder>(handleList.Count / 3); + var durationPerSegment = 1.0 / segments.Capacity; + for (var i = 0; i < segments.Capacity; i ++) { + var j = i * 3; + segments.Add(new BezierSegment( + durationPerSegment, + handleList[j], + handleList[j + 1], + handleList[j + 2], + handleList[j + 3] + )); } - if(segments.IsEmpty()) throw new ArgumentException("Only one handle passed. A bezier spline from cubic segments needs at least 4 handles"); - - return new BezierSpline(segments, ops); + return new BezierSpline(segments.MoveToImmutable(), ops); } /// - /// Constructs a spline that traverses each handle in order at the desired position and velocity. + /// Constructs a spline that traverses each handle in order at the desired position + /// and velocity and in the specified time. /// /// The positions the spline should visit along with the desired velocity at these points. /// The geometric operations necessary for calculation. + /// If true, the spline's trajectory is altered to ensure a + /// continuous acceleration. This is usually desirable for animations since jumps in the acceleration might + /// make the movement look less smooth. Since this makes the trajectory less predictable and increases the + /// computational load, you should not enable it unless you actually need it. + [Pure] + public static BezierSpline FromHandlesWithVelocities( + IEnumerable<(TPos Position, TVel Velocity, TTime Time)> handles, + GeometricOperations ops, + bool shouldAccelerationBeContinuous = false + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + if (shouldAccelerationBeContinuous) + return FromHandlesWithVelocitiesAndAccelerations( + handles.Select(h => (h.Position, h.Velocity, ops.ZeroVel, h.Time)), + ops + ); + else { + var segments = handles.PairwiseSliding().SelectTuple((left, right) => + BezierSegment.CubicWithVelocity( + left.Position, left.Velocity, + right.Position, right.Velocity, + ops.Sub(right.Time, left.Time), + ops)); + return new BezierSpline(segments, ops); + } + } + + /// + /// Constructs a spline that traverses each handle in order at the desired position + /// and tangent in uniform intervals. The spline has a duration of 1. + /// + /// The positions the spline should visit along with the desired tangent at these points. + /// The geometric operations necessary for calculation. /// Whether the spline should come back to the first handle or stop at the last. /// If true, the spline's trajectory is altered to ensure a /// continuous acceleration. This is usually desirable for animations since jumps in the acceleration might /// make the movement look less smooth. Since this makes the trajectory less predictable and increases the /// computational load, you should not enable it unless you actually need it. [Pure] - public static BezierSpline FromHandlesWithVelocities( - IEnumerable<(TPos position, TDiff velocity)> handles, - GeometricOperations ops, - bool shouldLoop = false, + public static BezierSpline UniformFromHandlesWithTangents( + IEnumerable<(TPos Position, TDiff Tangent)> handles, + GeometricOperations ops, + bool shouldLoop = false, bool shouldAccelerationBeContinuous = false ) where TPos : unmanaged where TDiff : unmanaged { if (shouldAccelerationBeContinuous) - return FromHandlesWithVelocitiesAndAccelerations( - handles.Select(h => (h.position, h.velocity, ops.ZeroDiff)), + return UniformFromHandlesWithTangentsAndCurvature( + handles.Select(h => (h.Position, h.Tangent, ops.ZeroDiff)), ops, shouldLoop ); else { - var (first, tail) = handles; - var allHandles = shouldLoop ? first.PrependTo(tail).Append(first) : first.PrependTo(tail); - var segments = allHandles.PairwiseSliding().SelectTuple((left, right) => - BezierSegment.Cubic(left.position, left.velocity, right.position, right.velocity, ops) - ); - return new BezierSpline(segments, ops); + var handleList = handles.AsReadOnlyList(); + var durationPerSegment = 1.0 / (shouldLoop ? handleList.Count + 1 : handleList.Count); + var segments = (shouldLoop ? handleList.Append(handleList[0]) : handleList).PairwiseSliding().SelectTuple((left, right) => + BezierSegment.CubicWithTangents( + left.Position, left.Tangent, + right.Position, right.Tangent, + durationPerSegment, + ops)); + return new BezierSpline(segments, ops); } } + + /// + /// Constructs a spline that traverses each handle in order at the desired position, + /// velocity and acceleration and in the specified time. + /// + /// The positions the spline should visit along with the desired velocity at these points. + /// The geometric operations necessary for calculation. + [Pure] + public static BezierSpline FromHandlesWithVelocitiesAndAccelerations( + IEnumerable<(TPos Position, TVel Velocity, TVel Acceleration, TTime Time)> handles, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + var segments = handles.PairwiseSliding().SelectTuple((left, right) => + BezierSegment.QuinticWithVelocities( + left.Position, left.Velocity, left.Acceleration, + right.Position, right.Velocity, right.Acceleration, + ops.Sub(right.Time, left.Time), + ops)); + return new BezierSpline(segments, ops); + } + + /// + /// Constructs a spline that traverses each handle in order at the desired position, + /// tangent and curvature and in uniform intervals. The spline has a duration of 1. + /// + /// The positions the spline should visit along with the desired tangent and curvature at these points. + /// The geometric operations necessary for calculation. + /// Whether the spline should come back to the first handle or stop at the last. [Pure] - public static BezierSpline FromHandlesWithVelocitiesAndAccelerations( - IEnumerable<(TPos position, TDiff velocity, TDiff acceleration)> handles, - GeometricOperations ops, + public static BezierSpline UniformFromHandlesWithTangentsAndCurvature( + IEnumerable<(TPos Position, TDiff Tangent, TDiff Curvature)> handles, + GeometricOperations ops, bool shouldLoop = false ) where TPos : unmanaged where TDiff : unmanaged { - var (first, tail) = handles; - var allHandles = shouldLoop ? first.PrependTo(tail).Append(first) : first.PrependTo(tail); - var segments = allHandles.PairwiseSliding().SelectTuple((left, right) => - BezierSegment.Quintic(left.position, left.velocity, left.acceleration, right.position, right.velocity, right.acceleration, ops) - ); - return new BezierSpline(segments, ops); + var handleList = handles.ToList(); + var durationPerSegment = 1.0 / (shouldLoop ? handleList.Count + 1 : handleList.Count); + var segments = (shouldLoop ? handleList.Append(handleList[0]) : handleList).PairwiseSliding().SelectTuple((left, right) => + BezierSegment.QuinticWithVelocities( + left.Position, left.Tangent, left.Curvature, + right.Position, right.Tangent, right.Curvature, + durationPerSegment, + ops)); + return new BezierSpline(segments, ops); } } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomPolynomial.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomPolynomial.cs index 89e6d25b..d9ada996 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomPolynomial.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomPolynomial.cs @@ -1,4 +1,5 @@ -using System.Diagnostics.Contracts; +using System; +using System.Diagnostics.Contracts; using BII.WasaBii.Core; using BII.WasaBii.Splines.Maths; @@ -7,9 +8,9 @@ namespace BII.WasaBii.Splines.CatmullRom { internal static class CatmullRomPolynomial { [Pure] - internal static Option> FromSplineAt(CatmullRomSpline spline, SplineSegmentIndex idx) - where TPos : unmanaged - where TDiff : unmanaged => + internal static Option> FromSplineAt( + CatmullRomSpline spline, SplineSegmentIndex idx + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => CatmullRomSegment.CatmullRomSegmentAt(spline, NormalizedSplineLocation.From(idx)) is { Segment: var segment } ? segment.ToPolynomial(spline.Type.ToAlpha()) : Option.None; diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSegment.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSegment.cs index fb6aa7fb..bd5e2314 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSegment.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSegment.cs @@ -16,54 +16,81 @@ namespace BII.WasaBii.Splines.CatmullRom { /// Describes the area between two spline handles (p1 and p2), /// with the supporting handles p0 and p3. /// - internal readonly struct CatmullRomSegment + internal readonly struct CatmullRomSegment where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged + where TVel : unmanaged + { public readonly TPos P0, P1, P2, P3; public TPos Start => P1; public TPos End => P2; - internal readonly GeometricOperations Ops; + public readonly TTime Duration; + public readonly TTime PrevDur; + public readonly TTime NextDur; + + internal readonly GeometricOperations Ops; - public CatmullRomSegment(TPos p0, TPos p1, TPos p2, TPos p3, GeometricOperations ops) { + public CatmullRomSegment(TPos p0, TPos p1, TPos p2, TPos p3, TTime duration, TTime prevDur, TTime nextDur, GeometricOperations ops) { P0 = p0; P1 = p1; P2 = p2; P3 = p3; Ops = ops; + Duration = duration; + PrevDur = prevDur; + NextDur = nextDur; + NextDur = nextDur; } [Pure] - internal Polynomial ToPolynomial(float alpha) { + internal Polynomial ToPolynomial(float alpha) { var ops = Ops; double DTFor(TPos pos1, TPos pos2, double orWhenZero) { var dt = Math.Pow(ops.Distance(pos1, pos2).AsMeters(), alpha); return dt < float.Epsilon ? orWhenZero : dt; } + + // Catmull-Rom splines are a special case of cubic hermite splines where the tangents + // at each segment endpoint are calculated by using the adjacent ("margin") control points. + // P1 and P2 are the segment endpoints, P0 and P3 are the adjacent control points. + // The tangents are m1 and m2. - var dt1 = DTFor(P1, P2, orWhenZero: 1.0f); - var dt0 = DTFor(P0, P1, orWhenZero: dt1); - var dt2 = DTFor(P2, P3, orWhenZero: dt1); + TDiff m1, m2; - TDiff TFor(TPos pa, TPos pb, TPos pc, double dta, double dtb) => - ops.Mul(ops.Add( - ops.Sub( - ops.Div(ops.Sub(pb, pa), dta), - ops.Div(ops.Sub(pc, pa), dta + dtb) - ), - ops.Div(ops.Sub(pc, pb), dtb) - ), dt1); + if (alpha == 0) { // uniform catmull-rom + m1 = ops.Div(ops.Sub(P2, P0), 2); + m2 = ops.Div(ops.Sub(P3, P1), 2); + } else { + var dt1 = DTFor(P1, P2, orWhenZero: 1.0f); + var dt0 = DTFor(P0, P1, orWhenZero: dt1); + var dt2 = DTFor(P2, P3, orWhenZero: dt1); + + TDiff TFor(TPos pa, TPos pb, TPos pc, double dta, double dtb) => + ops.Mul(ops.Add( + ops.Sub( + ops.Div(ops.Sub(pb, pa), dta), + ops.Div(ops.Sub(pc, pa), dta + dtb) + ), + ops.Div(ops.Sub(pc, pb), dtb) + ), dt1); - var t1 = TFor(P0, P1, P2, dt0, dt1); - var t2 = TFor(P1, P2, P3, dt1, dt2); + m1 = TFor(P0, P1, P2, dt0, dt1); + m2 = TFor(P1, P2, P3, dt1, dt2); + } + m1 = ops.Mul(m1, ops.Div(Duration, PrevDur)); + m2 = ops.Mul(m2, ops.Div(Duration, NextDur)); + return Polynomial.Cubic( a: P1, - b: t1, - c: ops.Sub(ops.Mul(ops.Sub(P2, P1), 3), ops.Mul(t1, 2), t2), - d: ops.Add(ops.Mul(ops.Sub(P1, P2), 2), t1, t2), + b: m1, + c: ops.Sub(ops.Mul(ops.Sub(P2, P1), 3), ops.Add(ops.Mul(m1, 2), m2)), + d: ops.Add(ops.Mul(ops.Sub(P1, P2), 2), ops.Add(m1, m2)), + Duration, ops ); } @@ -82,32 +109,38 @@ internal static class CatmullRomSegment { /// The exact position between P1 and P2 is determined by the returned NormalizedOvershoot: /// 0.0f is at P1, 1.0f is at P2 and the values in between are lerped. /// - public static (CatmullRomSegment Segment, double NormalizedOvershoot)? - CatmullRomSegmentAt(CatmullRomSpline spline, NormalizedSplineLocation location) - where TPos : unmanaged - where TDiff : unmanaged { + public static (CatmullRomSegment Segment, double NormalizedOvershoot)? + CatmullRomSegmentAt( + CatmullRomSpline spline, + NormalizedSplineLocation location + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { if(spline == null) throw new ArgumentNullException(nameof(spline)); if(double.IsNaN(location.Value)) throw new ArgumentException("The spline location is NaN", nameof(location)); - if (location < 0 || location > spline.SegmentCount + EndOfSplineOvershootTolerance) + if (location.Value < 0 || location.Value > spline.SegmentCount + EndOfSplineOvershootTolerance) return null; - var (s0, overshoot) = location >= spline.SegmentCount + var (s0, overshoot) = location.Value >= spline.SegmentCount // The location was almost at, or slightly above the end of the spline // but within tolerance. The used segment automatically // becomes the last valid catmull rom segment. ? (SplineHandleIndex.At(spline.SegmentCount - 1), 1.0f) // Otherwise the location is simply converted to a handle index and overshoot : location.AsHandleIndex(); + + var dur = spline.Ops.Sub(spline.TemporalSegmentOffsets[s0 + 1], spline.TemporalSegmentOffsets[s0]); - return (new CatmullRomSegment( + return (new CatmullRomSegment( spline[s0], spline[s0 + 1], spline[s0 + 2], spline[s0 + 3], + dur, + s0 <= 0 ? dur : spline.Ops.Sub(spline.TemporalSegmentOffsets[s0], spline.TemporalSegmentOffsets[s0 - 1]), + s0 >= spline.SegmentCount - 1 ? dur : spline.Ops.Sub(spline.TemporalSegmentOffsets[s0 + 2], spline.TemporalSegmentOffsets[s0 + 1]), spline.Ops ), overshoot); } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSpline.cs index 36b02a22..c3824310 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSpline.cs @@ -22,39 +22,64 @@ namespace BII.WasaBii.Splines.CatmullRom { /// segment, i.e. between two succinct points. /// [Serializable] - public sealed class CatmullRomSpline : Spline.Copyable where TPos : unmanaged where TDiff : unmanaged { + public sealed class CatmullRomSpline : Spline.Copyable + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged + { internal sealed record Cache( - ImmutableArray> SplineSegments, - ImmutableArray SegmentOffsetsFromBegin + ImmutableArray> SplineSegments, + Length Length, + ImmutableArray SpatialSegmentOffsets ); public CatmullRomSpline( - TPos startHandle, IEnumerable handles, TPos endHandle, - GeometricOperations ops, - SplineType? splineType = null - ) : this(handles.Prepend(startHandle).Append(endHandle), ops, splineType) {} + TPos startMarginHandle, + IEnumerable<(TPos, TTime)> handles, + TPos endMarginHandle, + GeometricOperations ops, + SplineType splineType = SplineType.Centripetal + ) { + + // Note CR: Serialization might pass only `default` parameters, so we support this case here + if (handles != default) { + var (handlePositions, handleTimes) = handles.Unzip(); + if (handlePositions.Count < 2) + throw new InsufficientNodePositionsException(actual: handlePositions.Count, required: 2); + var handlesBuilder = ImmutableArray.CreateBuilder(handlePositions.Count + 2); + handlesBuilder.Add(startMarginHandle); + handlesBuilder.AddRange(handlePositions); + handlesBuilder.Add(endMarginHandle); + this.handles = handlesBuilder.MoveToImmutable(); + this.TemporalSegmentOffsets = ImmutableArray.CreateRange(handleTimes); + } + Type = splineType; + cache = new Lazy(initCache); + this.Ops = ops; + } public CatmullRomSpline( - IEnumerable allHandlesIncludingMarginHandles, - GeometricOperations ops, - SplineType? splineType = null + IEnumerable allHandlesIncludingMarginHandles, + IEnumerable handleTimes, + GeometricOperations ops, + SplineType splineType = SplineType.Centripetal ) { // Note CR: Serialization might pass only `default` parameters, so we support this case here - if (allHandlesIncludingMarginHandles != default) { - handles = ImmutableArray.CreateRange(allHandlesIncludingMarginHandles); + if (allHandlesIncludingMarginHandles != default && handleTimes != null) { + handles = allHandlesIncludingMarginHandles.ToImmutableArray(); + TemporalSegmentOffsets = handleTimes.ToImmutableArray(); if (handles.Length < 4) - throw new ArgumentException( - $"Cannot construct a Catmull-Rom spline from {handles.Length} handles, at least 4 are needed" - ); + throw new InsufficientNodePositionsException(actual: handles.Length, required: 4); + if (TemporalSegmentOffsets.Length + 2 != handles.Length) + throw new ArgumentException("Number of timestamps does not match number of non-margin handles"); } else handles = ImmutableArray.Empty; - Type = splineType ?? SplineType.Centripetal; + Type = splineType; cache = new Lazy(initCache); this.Ops = ops; } private readonly ImmutableArray handles; + public ImmutableArray TemporalSegmentOffsets { get; } public IReadOnlyList HandlesIncludingMargin => handles; @@ -65,42 +90,45 @@ public CatmullRomSpline( ); public int SegmentCount => HandlesIncludingMargin.Count - 3; + public Length Length => cache.Value.Length; + public TTime TotalDuration => TemporalSegmentOffsets[^1]; + public SplineType Type { get; } - public GeometricOperations Ops { get; } + public GeometricOperations Ops { get; } - public IEnumerable> Segments => cache.Value.SplineSegments; + public IEnumerable> Segments => cache.Value.SplineSegments; public TPos this[SplineHandleIndex index] => handles[index]; - public SplineSample this[SplineLocation location] => this[this.NormalizeOrThrow(location)]; - - public SplineSegment this[SplineSegmentIndex index] => cache.Value.SplineSegments[index]; - - public SplineSample this[NormalizedSplineLocation location] => - SplineSample.From(this, location).GetOrThrow(() => + public SplineSample this[TTime t] => + SplineSample.From(this, t); + public SplineSegment this[SplineSegmentIndex index] => cache.Value.SplineSegments[index]; + public SplineSample this[SplineLocation location] => this[this.NormalizeOrThrow(location)]; + public SplineSample this[NormalizedSplineLocation location] => + SplineSample.From(this, location).GetOrThrow(() => new ArgumentOutOfRangeException( nameof(location), location, $"Must be between 0 and {SegmentCount}" )); - public ImmutableArray SegmentOffsetsFromBegin => cache.Value.SegmentOffsetsFromBegin; + public ImmutableArray SpatialSegmentOffsets => cache.Value.SpatialSegmentOffsets; - [Pure] public Spline Map( - Func positionMapping, GeometricOperations newOps - ) where TPosNew : unmanaged where TDiffNew : unmanaged => - new CatmullRomSpline(HandlesIncludingMargin.Select(positionMapping), newOps, Type); + [Pure] public Spline Map( + Func positionMapping, GeometricOperations newOps + ) where TPosNew : unmanaged where TDiffNew : unmanaged where TVelNew : unmanaged => + new CatmullRomSpline(HandlesIncludingMargin.Select(positionMapping), TemporalSegmentOffsets, newOps, Type); - [Pure] public Spline Reversed => new CatmullRomSpline(HandlesIncludingMargin.Reverse(), Ops, Type); + [Pure] public Spline Reversed => new CatmullRomSpline(HandlesIncludingMargin.Reverse(), TemporalSegmentOffsets, Ops, Type); - [Pure] public Spline CopyWithOffset(Func tangentToOffset) => + [Pure] public Spline CopyWithOffset(Func tangentToOffset) => CatmullRomSplineCopyUtils.CopyWithOffset(this, tangentToOffset); - [Pure] public Spline CopyWithStaticOffset(TDiff offset) => + [Pure] public Spline CopyWithStaticOffset(TDiff offset) => CatmullRomSplineCopyUtils.CopyWithStaticOffset(this, offset); - [Pure] public Spline CopyWithDifferentHandleDistance(Length desiredHandleDistance) => + [Pure] public Spline CopyWithDifferentHandleDistance(Length desiredHandleDistance) => CatmullRomSplineCopyUtils.CopyWithDifferentHandleDistance(this, desiredHandleDistance); #region Segment Length Caching @@ -110,8 +138,8 @@ [Pure] public Spline CopyWithDifferentHandleDistance(Length desired private Cache initCache() { var segmentCount = SegmentCount; - var segments = ImmutableArray.CreateBuilder>(initialCapacity: segmentCount); - for(var i = 0; i< segmentCount; i++) segments.Add(new SplineSegment( + var segments = ImmutableArray.CreateBuilder>(initialCapacity: segmentCount); + for(var i = 0; i< segmentCount; i++) segments.Add(new SplineSegment( CatmullRomPolynomial.FromSplineAt(this, SplineSegmentIndex.At(i)) .GetOrThrow(() => new Exception( @@ -119,12 +147,19 @@ private Cache initCache() { "This should not happen and indicates a bug in this method." ) ))); - var segmentOffsets = ImmutableArray.CreateBuilder(initialCapacity: segmentCount); + + var spatialOffsets = ImmutableArray.CreateBuilder(initialCapacity: segmentCount); + var lastOffset = Length.Zero; - segmentOffsets.Add(Length.Zero); - for (var i = 1; i < segmentCount; i++) - segmentOffsets.Add(lastOffset += segments[i - 1].Length); - return new(segments.MoveToImmutable(), segmentOffsets.MoveToImmutable()); + foreach (var segment in segments) { + if (segment.Duration.CompareTo(Ops.ZeroTime) == -1) + throw new Exception( + $"Tried to construct a spline with a segment of negative duration: {segment.Duration}" + ); + spatialOffsets.Add(lastOffset); + lastOffset += segment.Length; + } + return new(segments.MoveToImmutable(), lastOffset, spatialOffsets.MoveToImmutable()); } #endregion @@ -151,20 +186,20 @@ public NotEnoughHandles(int handlesProvided, int handlesNeeded) { } } - public static TPos BeginMarginHandle(this CatmullRomSpline spline) - where TPos : unmanaged where TDiff : unmanaged => + public static TPos BeginMarginHandle(this CatmullRomSpline spline) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.HandlesIncludingMargin[0]; - public static TPos EndMarginHandle(this CatmullRomSpline spline) - where TPos : unmanaged where TDiff : unmanaged => + public static TPos EndMarginHandle(this CatmullRomSpline spline) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.HandlesIncludingMargin[^1]; - public static TPos FirstHandle(this CatmullRomSpline spline) - where TPos : unmanaged where TDiff : unmanaged => + public static TPos FirstHandle(this CatmullRomSpline spline) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.Handles[0]; - public static TPos LastHandle(this CatmullRomSpline spline) - where TPos : unmanaged where TDiff : unmanaged => + public static TPos LastHandle(this CatmullRomSpline spline) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.Handles[^1]; /// @@ -176,27 +211,30 @@ public static TPos LastHandle(this CatmullRomSpline sp /// while this operation is only done twice here. /// [Pure] - public static IEnumerable HandlesBetween( - this CatmullRomSpline spline, SplineLocation start, SplineLocation end - ) where TPos : unmanaged where TDiff : unmanaged { + public static IEnumerable HandlesBetween( + this CatmullRomSpline spline, SplineLocation start, SplineLocation end + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { var fromNormalized = spline.NormalizeOrThrow(start); var toNormalized = spline.NormalizeOrThrow(end); - yield return spline[fromNormalized].Position; - if (fromNormalized > toNormalized) { - // Iterate from end of spline to begin - for (var nodeIndex = spline.Handles.Count - 1; nodeIndex >= 0; --nodeIndex) - if (nodeIndex < fromNormalized && nodeIndex > toNormalized) - yield return spline.Handles[nodeIndex]; + var offset = MathD.CeilToInt(toNormalized.Value); + var count = MathD.FloorToInt(fromNormalized.Value) - offset; + return new ReadOnlyListSegment( + spline.Handles, + offset, + count + ).ReverseList() + .Prepend(spline[fromNormalized].Position).Append(spline[toNormalized].Position); } else { - // Iterate from begin of spline to end - for (var nodeIndex = 0; nodeIndex < spline.Handles.Count; ++nodeIndex) - if (nodeIndex > fromNormalized && nodeIndex < toNormalized) - yield return spline.Handles[nodeIndex]; + var offset = MathD.CeilToInt(fromNormalized.Value); + var count = MathD.FloorToInt(toNormalized.Value) - offset; + return new ReadOnlyListSegment( + spline.Handles, + offset, + count + ).Prepend(spline[fromNormalized].Position).Append(spline[toNormalized].Position); } - - yield return spline[toNormalized].Position; } } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineCopyUtils.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineCopyUtils.cs index 2c34fd61..b8ee6fe0 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineCopyUtils.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineCopyUtils.cs @@ -1,5 +1,6 @@ using System; using System.Linq; +using BII.WasaBii.Core; using BII.WasaBii.UnitSystem; namespace BII.WasaBii.Splines.CatmullRom { @@ -9,23 +10,23 @@ public static class CatmullRomSplineCopyUtils { /// Creates a new spline with a similar trajectory as , but with all handle /// positions being moved by a certain offset which depends on the spline's tangent at these points. /// - public static CatmullRomSpline CopyWithOffset( - CatmullRomSpline original, Func tangentToOffset - ) where TPos : unmanaged where TDiff : unmanaged { + public static CatmullRomSpline CopyWithOffset( + CatmullRomSpline original, Func tangentToOffset + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { TPos computePosition( - Spline deriveFrom, TPos originalPosition, NormalizedSplineLocation tangentLocation - ) => original.Ops.Sub(originalPosition, tangentToOffset(deriveFrom[tangentLocation].Tangent)); + Spline deriveFrom, TPos originalPosition, NormalizedSplineLocation tangentLocation + ) => original.Ops.Sub(originalPosition, tangentToOffset(deriveFrom[tangentLocation].Velocity)); - return new CatmullRomSpline( - computePosition(original, original.BeginMarginHandle(), NormalizedSplineLocation.Zero), - original.Handles.Select((node, idx) => - computePosition(original, node, NormalizedSplineLocation.From(idx)) - ), - computePosition( - original, - original.EndMarginHandle(), - NormalizedSplineLocation.From(original.Handles.Count - 1) - ), + return new CatmullRomSpline( + computePosition(original, original.BeginMarginHandle(), NormalizedSplineLocation.Zero) + .PrependTo(original.Handles.Select((node, idx) => + computePosition(original, node, NormalizedSplineLocation.From(idx)) + )).Append(computePosition( + original, + original.EndMarginHandle(), + NormalizedSplineLocation.From(original.Handles.Count - 1) + )), + original.TemporalSegmentOffsets, original.Ops, original.Type ); @@ -37,30 +38,31 @@ TPos computePosition( /// being moved along a certain , /// independent of the spline's tangent at these points. /// - public static CatmullRomSpline CopyWithStaticOffset( - CatmullRomSpline original, TDiff offset - ) where TPos : unmanaged where TDiff : unmanaged { + public static CatmullRomSpline CopyWithStaticOffset( + CatmullRomSpline original, TDiff offset + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { TPos computePosition(TPos node) => original.Ops.Add(node, offset); - return new CatmullRomSpline( - computePosition(original.BeginMarginHandle()), - original.Handles.Select(computePosition), - computePosition(original.EndMarginHandle()), + return new CatmullRomSpline( + computePosition(original.BeginMarginHandle()) + .PrependTo(original.Handles.Select(computePosition)) + .Append(computePosition(original.EndMarginHandle())), + original.TemporalSegmentOffsets, original.Ops, original.Type ); } /// - /// Creates a new spline with a similar trajectory as - /// , but different spacing - /// between the non-margin handles. + /// Creates a new spline with a similar trajectory as , but + /// with a uniform spacing of between the handles. /// - public static CatmullRomSpline CopyWithDifferentHandleDistance( - CatmullRomSpline original, Length desiredHandleDistance - ) where TPos : unmanaged where TDiff : unmanaged => + /// The velocities at the new handles are preserved, the accelerations are not. + public static CatmullRomSpline CopyWithDifferentHandleDistance( + CatmullRomSpline original, Length desiredHandleDistance + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => new( original.Ops.Lerp(original.Handles[0], original.BeginMarginHandle(), desiredHandleDistance / original.Ops.Distance(original.Handles[0], original.BeginMarginHandle())), - original.SampleSplineEvery(desiredHandleDistance).Select(sample => sample.Position), + original.SampleSplineEvery(desiredHandleDistance).Select(sample => (sample.Position, sample.GlobalT)), original.Ops.Lerp(original.Handles[^1], original.EndMarginHandle(), desiredHandleDistance / original.Ops.Distance(original.Handles[^1], original.EndMarginHandle())), original.Ops, original.Type diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineFactory.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineFactory.cs index 837cfb25..e62343fa 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineFactory.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/CatmullRom/CatmullRomSplineFactory.cs @@ -1,4 +1,6 @@ -using System.Collections.Generic; +using System; +using System.Collections.Generic; +using System.Collections.Immutable; using System.Diagnostics.Contracts; using System.Linq; using BII.WasaBii.Core; @@ -8,13 +10,38 @@ namespace BII.WasaBii.Splines.CatmullRom { /// /// Utilities for constructing generic catmull-rom splines. - /// For explicitly typed variants with + /// For explicitly typed variants with /// included, use `UnitySpline`, `GlobalSpline` or `LocalSpline` in the Unity assembly. /// public static partial class CatmullRomSpline { /// - /// Creates a catmull-rom spline from the provided positions. + /// Creates a non-uniform catmull-rom spline from the provided positions + /// and the respective times at which they should be traversed. + /// The begin and end margin handles are not interpolated by + /// the spline and merely affect its trajectory at the spline's + /// start and end. + /// + /// This should be used when the trajectory at the spline's begin / end + /// needs to be clearly defined. + /// + /// if less than 2 interpolated handle positions were provided + [Pure] + public static Result, NotEnoughHandles> FromHandles( + TPos beginMarginHandle, + IEnumerable<(TPos, TTime)> interpolatedHandlesAndTimeSteps, + TPos endMarginHandle, + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + var handles = interpolatedHandlesAndTimeSteps.AsReadOnlyList(); + return handles.Count >= 2 + ? new CatmullRomSpline(beginMarginHandle, handles, endMarginHandle, ops, type) + : new NotEnoughHandles(handles.Count + 2, 4); + } + + /// + /// Creates a uniform catmull-rom spline from the provided positions. /// The begin and end margin handles are not interpolated by /// the spline and merely affect its trajectory at the spline's /// start and end. @@ -25,21 +52,49 @@ public static partial class CatmullRomSpline { /// /// if less than 2 interpolated handle positions were provided [Pure] - public static Result, NotEnoughHandles> FromHandles( + public static Result, NotEnoughHandles> UniformFromHandles( TPos beginMarginHandle, IEnumerable interpolatedHandles, TPos endMarginHandle, - GeometricOperations ops, - SplineType? type = null - ) where TPos : unmanaged where TDiff : unmanaged => - FromHandlesIncludingMargin( - interpolatedHandles.Prepend(beginMarginHandle).Append(endMarginHandle), - ops, - type - ); + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged { + var handles = interpolatedHandles.AsReadOnlyList(); + var durationPerSegment = 1.0 / (handles.Count - 1); + return handles.Count >= 2 + ? new CatmullRomSpline( + beginMarginHandle.PrependTo(handles).Append(endMarginHandle), + Enumerable.Repeat(durationPerSegment, handles.Count - 1), + ops, + type) + : new NotEnoughHandles(handles.Count + 2, 4); + } + + /// + /// Creates a non-uniform catmull-rom spline from the provided positions + /// and the respective times at which they should be traversed. + /// The begin and end margin handles are not interpolated by + /// the spline and merely affect its trajectory at the spline's + /// start and end. + /// + /// This should be used when the trajectory at the spline's begin / end + /// needs to be clearly defined. + /// + /// + /// When less than 2 interpolated handle positions were provided + /// + [Pure] + public static CatmullRomSpline FromHandlesOrThrow( + TPos beginMarginHandle, + IEnumerable<(TPos, TTime)> interpolatedHandlesAndTimeSteps, + TPos endMarginHandle, + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + new(beginMarginHandle, interpolatedHandlesAndTimeSteps, endMarginHandle, ops, type); /// - /// Creates a catmull-rom spline from the provided positions. + /// Creates a uniform catmull-rom spline from the provided positions. /// The begin and end margin handles are not interpolated by /// the spline and merely affect its trajectory at the spline's /// start and end. @@ -52,23 +107,29 @@ public static Result, NotEnoughHandles> FromHandle /// When less than 2 interpolated handle positions were provided /// [Pure] - public static CatmullRomSpline FromHandlesOrThrow( + public static CatmullRomSpline UniformFromHandlesOrThrow( TPos beginMarginHandle, IEnumerable interpolatedHandles, TPos endMarginHandle, - GeometricOperations ops, - SplineType? type = null - ) where TPos : unmanaged where TDiff : unmanaged => - FromHandlesIncludingMarginOrThrow( - interpolatedHandles.Prepend(beginMarginHandle).Append(endMarginHandle), - ops, - type - ); + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged { + var handles = ImmutableArray.CreateBuilder(); + handles.Add(beginMarginHandle); + handles.AddRange(interpolatedHandles); + handles.Add(endMarginHandle); + if (handles.Count < 4) + throw new InsufficientNodePositionsException(handles.Count - 2, 2); + var segmentCount = handles.Count - 3; + var times = ImmutableArray.CreateBuilder(segmentCount + 1); + times.AddRange(Enumerable.Range(0, segmentCount + 1).Select(i => i / (double) segmentCount)); + return new(handles.MoveToImmutable(), times.MoveToImmutable(), ops, type); + } /// - /// Creates a catmull-rom spline that interpolates the provided positions. + /// Creates a non-uniform catmull-rom spline that interpolates the provided positions at the provided times. /// The margin handles of the spline are created automatically - /// using . + /// using . /// /// This should be used when the trajectory at the spline's begin / end /// should just be similar to the trajectory of the rest of the spline. @@ -76,40 +137,85 @@ public static CatmullRomSpline FromHandlesOrThrow( /// /// When less than 2 handle positions were provided /// - public static CatmullRomSpline FromHandlesOrThrow( - IEnumerable source, GeometricOperations ops, SplineType? splineType = null, bool shouldLoop = false - ) where TPos : unmanaged where TDiff : unmanaged { - var positions = source.AsReadOnlyCollection(); + public static CatmullRomSpline FromHandlesOrThrow( + IEnumerable<(TPos, TTime)> source, GeometricOperations ops, SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + FromHandles(source, ops, type).ResultOrThrow(notEnoughHandles => + new InsufficientNodePositionsException(notEnoughHandles.HandlesProvided, notEnoughHandles.HandlesNeeded)); + + /// + /// Creates a uniform catmull-rom spline that interpolates the provided positions. + /// The margin handles of the spline are created automatically + /// using . + /// + /// This should be used when the trajectory at the spline's begin / end + /// should just be similar to the trajectory of the rest of the spline. + /// + /// + /// When less than 2 handle positions were provided + /// + public static CatmullRomSpline UniformFromHandlesOrThrow( + IEnumerable source, GeometricOperations ops, SplineType type = SplineType.Centripetal, bool shouldLoop = false + ) where TPos : unmanaged where TDiff : unmanaged => + UniformFromHandles(source, ops, type, shouldLoop).ResultOrThrow(notEnoughHandles => + new InsufficientNodePositionsException(notEnoughHandles.HandlesProvided, notEnoughHandles.HandlesNeeded)); + + /// + /// Creates a non-uniform catmull-rom spline that interpolates the provided positions at the provided times. + /// The margin handles of the spline are created automatically + /// using . + /// + /// This should be used when the trajectory at the spline's begin / end + /// should just be similar to the trajectory of the rest of the spline. + /// + /// Returns if too few positions are provided. + /// + public static Result, NotEnoughHandles> FromHandles( + IEnumerable<(TPos, TTime)> source, GeometricOperations ops, SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + var (positions, times) = source.Unzip(); if (positions.Count < 2) - throw new InsufficientNodePositionsException(positions.Count, 2); + return new NotEnoughHandles(positions.Count, 2); - var handles = shouldLoop ? positions.Append(positions.First()) : positions; - var (beginHandle, endHandle) = handles.calculateSplineMarginHandles(ops, shouldLoop); - return new CatmullRomSpline(beginHandle, handles, endHandle, ops, splineType); + var (beginHandle, endHandle) = positions.calculateSplineMarginHandles(ops, shouldLoop: false); + var allHandles = ImmutableArray.CreateBuilder(positions.Count + 2); + allHandles.Add(beginHandle); + allHandles.AddRange(positions); + allHandles.Add(endHandle); + return new CatmullRomSpline(allHandles.MoveToImmutable(), times, ops, type); } /// - /// Creates a catmull-rom spline that interpolates the provided positions. + /// Creates a uniform catmull-rom spline that interpolates the provided positions. /// The margin handles of the spline are created automatically - /// using . + /// using . /// /// This should be used when the trajectory at the spline's begin / end /// should just be similar to the trajectory of the rest of the spline. /// /// Returns if too few positions are provided. /// - public static Result, NotEnoughHandles> FromHandles( - IEnumerable source, GeometricOperations ops, SplineType? type = null, bool shouldLoop = false + public static Result, NotEnoughHandles> UniformFromHandles( + IEnumerable source, GeometricOperations ops, SplineType type = SplineType.Centripetal, bool shouldLoop = false ) where TPos : unmanaged where TDiff : unmanaged { var positions = source.AsReadOnlyCollection(); - if (positions.Count < 2) return new NotEnoughHandles(positions.Count, 2); - var handles = shouldLoop ? positions.Append(positions.First()) : positions; - var (beginHandle, endHandle) = handles.calculateSplineMarginHandles(ops, shouldLoop); - return new CatmullRomSpline(beginHandle, handles, endHandle, ops, type); + if (positions.Count < 2) + return new NotEnoughHandles(positions.Count, 2); + + var (beginHandle, endHandle) = positions.calculateSplineMarginHandles(ops, shouldLoop); + var allHandles = ImmutableArray.CreateBuilder(positions.Count + (shouldLoop ? 3 : 2)); + allHandles.Add(beginHandle); + allHandles.AddRange(positions); + if(shouldLoop) allHandles.Add(positions.First()); + allHandles.Add(endHandle); + var segmentCount = allHandles.Count - 3; + var times = ImmutableArray.CreateBuilder(segmentCount + 1); + times.AddRange(Enumerable.Range(0, segmentCount + 1).Select(i => i / (double) segmentCount)); + return new CatmullRomSpline(allHandles.MoveToImmutable(), times.MoveToImmutable(), ops, type); } /// - /// Tries to create a catmull-rom spline from the provided positions. + /// Creates a non-uniform catmull-rom spline that interpolates the provided positions at the provided times. /// The first and last position of become the begin and end handles, /// which means that they are not interpolated by the spline and merely affect its /// trajectory at the spline's start and end. @@ -117,24 +223,52 @@ public static Result, NotEnoughHandles> FromHandle /// This should be used when the trajectory at the spline's begin / end /// needs to be clearly defined. /// - /// if has less than - /// 4 entries. + /// if has less than 4 entries. + /// if the amount f segment start times does not match the segment count [Pure] - public static Result, NotEnoughHandles> FromHandlesIncludingMargin( - IEnumerable allHandlesIncludingMargin, - GeometricOperations ops, - SplineType? type = null - ) where TPos : unmanaged where TDiff : unmanaged { + public static Result, NotEnoughHandles> FromHandlesIncludingMargin( + IEnumerable allHandlesIncludingMargin, + IEnumerable segmentStartTimes, + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { var positions = allHandlesIncludingMargin.AsReadOnlyCollection(); + var times = segmentStartTimes.AsReadOnlyCollection(); + if (times.Count != positions.Count - 3) + throw new ArgumentException("Amount of segment start times does not match segment count"); return Result.If( positions.Count >= 4, - () => new CatmullRomSpline(positions, ops, type), + () => new CatmullRomSpline(positions, times, ops, type), () => new NotEnoughHandles(positions.Count, 4) ); } /// - /// Creates a catmull-rom spline from the provided positions. + /// Tries to create a uniform catmull-rom spline from the provided positions. + /// The first and last position of become the begin and end handles, + /// which means that they are not interpolated by the spline and merely affect its + /// trajectory at the spline's start and end. + /// + /// This should be used when the trajectory at the spline's begin / end + /// needs to be clearly defined. + /// + /// if has less than 4 entries. + [Pure] + public static Result, NotEnoughHandles> UniformFromHandlesIncludingMargin( + IEnumerable allHandlesIncludingMargin, + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged { + var positions = allHandlesIncludingMargin.AsReadOnlyCollection(); + if (positions.Count < 4) return new NotEnoughHandles(positions.Count, 4); + var segmentCount = positions.Count - 3; + var times = ImmutableArray.CreateBuilder(segmentCount + 1); + times.AddRange(Enumerable.Range(0, segmentCount + 1).Select(i => i / (double) segmentCount)); + return new CatmullRomSpline(positions, times.MoveToImmutable(), ops, type); + } + + /// + /// Creates a non-uniform catmull-rom spline from the provided positions at the provided times. /// The first and last position of become the begin and end handles, /// which means that they are not interpolated by the spline and merely affect its /// trajectory at the spline's start and end. @@ -146,13 +280,37 @@ public static Result, NotEnoughHandles> FromHandle /// /// When less than 4 handle positions were provided /// + /// if the amount f segment start times does not match the segment count [Pure] - public static CatmullRomSpline FromHandlesIncludingMarginOrThrow( + public static CatmullRomSpline FromHandlesIncludingMarginOrThrow( IEnumerable allHandlesIncludingMargin, - GeometricOperations ops, - SplineType? type = null - ) where TPos : unmanaged where TDiff : unmanaged - => FromHandlesIncludingMargin(allHandlesIncludingMargin, ops, type) + IEnumerable segmentStartTimes, + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + FromHandlesIncludingMargin(allHandlesIncludingMargin, segmentStartTimes, ops, type) + .ResultOrThrow(error => new InsufficientNodePositionsException(error.HandlesProvided, 4)); + + /// + /// Creates a uniform catmull-rom spline from the provided positions. + /// The first and last position of become the begin and end handles, + /// which means that they are not interpolated by the spline and merely affect its + /// trajectory at the spline's start and end. + /// + /// This should be used when the trajectory at the spline's begin / end + /// needs to be clearly defined. + /// + /// + /// + /// When less than 4 handle positions were provided + /// + [Pure] + public static CatmullRomSpline UniformFromHandlesIncludingMarginOrThrow( + IEnumerable allHandlesIncludingMargin, + GeometricOperations ops, + SplineType type = SplineType.Centripetal + ) where TPos : unmanaged where TDiff : unmanaged => + UniformFromHandlesIncludingMargin(allHandlesIncludingMargin, ops, type) .ResultOrThrow(error => new InsufficientNodePositionsException(error.HandlesProvided, 4)); /// @@ -167,9 +325,9 @@ public static CatmullRomSpline FromHandlesIncludingMarginOrThrow /// When less than 2 handle positions were provided /// - private static (TPos BeginHandle, TPos EndHandle) calculateSplineMarginHandles( - this IEnumerable handlePositions, GeometricOperations ops, bool shouldLoop - ) where TPos : unmanaged where TDiff : unmanaged { + private static (TPos BeginHandle, TPos EndHandle) calculateSplineMarginHandles( + this IEnumerable handlePositions, GeometricOperations ops, bool shouldLoop + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged { var positions = handlePositions.AsReadOnlyList(); if (positions.Count < 2) throw new InsufficientNodePositionsException(positions.Count, 2); @@ -179,9 +337,9 @@ private static (TPos BeginHandle, TPos EndHandle) calculateSplineMarginHandles(this TPos self, TPos on, GeometricOperations ops) - where TPos : unmanaged where TDiff : unmanaged - => ops.Add(on, ops.Sub(on, self)); + private static TPos pointReflect(this TPos self, TPos on, GeometricOperations ops) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged => + ops.Add(on, ops.Sub(on, self)); } } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineExtensions.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineExtensions.cs index f219dadf..516c466f 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineExtensions.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineExtensions.cs @@ -1,125 +1,79 @@ using System; -using System.Diagnostics.Contracts; using BII.WasaBii.Core; namespace BII.WasaBii.Splines { public static class ClosestOnSplineExtensions { - public const int DefaultClosestOnSplineSamples = 5; + + public const int DefaultInitialSamplingCount = 10; + public const int DefaultIterations = 10; + public const double DefaultMinStepSize = 0.005; /// - /// Equal to , - /// but a non-al result is returned. - /// Throws when the provided spline is invalid. + /// Tries to find the location on the spline where its position has the minimum Euclidean distance to the given . + /// Works by first sampling the spline at points and using the best one as a starting point. + /// Then, it iteratively refines the result by using the Newton method, with a maximum of iterations. + /// Returns early when the individual step size is smaller than . + ///
+ /// More initial samples and more iterations lead to higher accuracy, but also higher computational cost. + /// Increasing the number of iterations increases the chance of finding a minimum. + /// Increasing the number of initial samples means that fewer iterations are needed and reduces the chance of getting stuck in a local minimum. + /// Complex splines with a lot of turns require more initial samples than simple straight-ish ones. /// - /// - /// Determines the accuracy of the query. Higher values lead to higher accuracy. - /// However, the default value should be sufficient for all cases. - /// - /// - /// This method should only be used on splines where the distance - /// between pairs of handles is approximately the same. - /// This is because the errorMarginNormalized is relative to the - /// length of a segment between spline handles. - /// Therefore differing distances between handles would lead to different - /// querying accuracies on different points on the spline. - /// - [Pure] public static ClosestOnSplineQueryResult QueryClosestPositionOnSplineToOrThrow( - this Spline spline, + public static ClosestOnSplineQueryResult QueryClosestPositionOnSplineTo( + this Spline spline, TPos position, - int samples = DefaultClosestOnSplineSamples - ) where TPos : unmanaged where TDiff : unmanaged => - spline.QueryClosestPositionOnSplineTo(position, samples).GetOrThrow(() => - new ArgumentException( - $"The spline given to {nameof(QueryClosestPositionOnSplineToOrThrow)} was not valid and a query could therefore not be performed!" - )); + int initialSamples = DefaultInitialSamplingCount, + int iterations = DefaultIterations, + double minStepSize = DefaultMinStepSize + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + var ops = spline.Ops; + + if(initialSamples < 1) throw new ArgumentException("The number of initial samples must be at least 1."); + + // Take a number of samples along the spline and take the best one -> attempt to find the area of the global minimum + var closest = spline.SampleSpline(initialSamples) + .MinBy(sample => ops.Distance(sample.Position, position)) + .GetOrThrow(); // should not throw if initialSamples > 0 - /// - /// This function returns the closest location and position (with its distance to the provided position) - /// on the spline, relative to the provided position. - /// It is greedy, which means that on heavily curved splines it will not find the global optimal solution - /// - /// - /// Determines the accuracy of the query. Higher values lead to higher accuracy. - /// However, the default value should be sufficient for all cases. - /// - /// - /// This method should only be used on splines where the distance - /// between pairs of handles is approximately the same. - /// This is because the errorMarginNormalized is relative to the - /// length of a segment between spline handles. - /// Therefore differing distances between handles would lead to different - /// querying accuracies on different points on the spline. - /// - public static Option> QueryClosestPositionOnSplineTo( - this Spline spline, - TPos position, - int samples = DefaultClosestOnSplineSamples - ) where TPos : unmanaged where TDiff : unmanaged { - - // 0: The position is on the plane, - // > 0: The position is above the plane (in the direction of the normal) - // < 0: The position is below the plane (opposite direction of the normal) - double compareToPlane(TPos planePosition, TDiff planeNormal) => - spline.Ops.Dot(spline.Ops.Sub(position, planePosition), planeNormal); - - ClosestOnSplineQueryResult computeResult( - TPos closestPosition, NormalizedSplineLocation closestLocation - ) => new( - position, - spline, - closestPosition, - closestLocation - ); - - var totalIntervals = spline.SegmentCount; - var lower = 0; - var upper = totalIntervals; - - // Binary search: We find the normalized spline location segment [lower, upper] where upper = lower + 1 - // in which the queriedPosition is located. - while (upper - lower > 1) { - var currentLocation = (upper + lower) / 2; // Intentional integer result - var (pos, tan) = spline[NormalizedSplineLocation.From(currentLocation)].PositionAndTangent; - var comparison = compareToPlane(pos, tan); + // we need to scale the derivatives later, depending on what temporal proportion of the spline a segment has + var durationScaling = ops.Div(spline.TotalDuration, closest.Segment.Duration); + var currentSegment = closest.SegmentIndex; + + // Iteratively refine the result using the newton method + for (var i = 0; i < iterations; i++) { + // We want to minimize f(t) = ||spline[t].pos - position||^2 + // where t is the normalized location + // Refining with newton's method means updating the location like this: + // t = t - f'(t) / f''(t) + // where + // f'(t) = 2 * dot(spline[t].pos - position, spline[t].deriv) + // f''(t) = 2 * (dot(spline[t].deriv, spline[t].deriv) + dot(spline[t].pos - position, spline[t].deriv2)) + + var p = closest.Position; + var pDeriv = ops.Mul(closest.DerivativeInSegment, durationScaling); + var pDeriv2 = ops.Mul(closest.SecondDerivativeInSegment, durationScaling); - if (Math.Abs(comparison) < float.Epsilon) { - // Early exit: The query position is exactly on the plane of the current location, - // therefore that location is the closest result - return computeResult(pos, NormalizedSplineLocation.From(currentLocation)); - } else if (comparison > 0) { - lower = SplineSegmentIndex.At(currentLocation); - } else { - upper = SplineSegmentIndex.At(currentLocation); - } - } - - (TPos position, TDiff tangent) getPositionAndTangentAtNormalized( - NormalizedSplineLocation location - ) => spline[location].PositionAndTangent; + // we skip the "2 * " part as it cancels out + var diff = ops.Sub(p, position); + var fDeriv = ops.Dot(diff, pDeriv); + var fDeriv2 = ops.Dot(pDeriv, pDeriv) + ops.Dot(diff, pDeriv2); + + var dDiff = fDeriv / fDeriv2; + + // Apply the new t (and clamp) + var oldT = closest.NormalizedLocation; + var newT = oldT - dDiff; + if(newT.Value < 0) { newT = new NormalizedSplineLocation(0); } + if(newT.Value > spline.SegmentCount) newT = new NormalizedSplineLocation(spline.SegmentCount); + closest = spline[newT]; - // Edge case: If the queriedPosition is inside the first segment and comes before it, the closest location is 0 - if (lower == 0) { - var (pos, tan) = getPositionAndTangentAtNormalized(NormalizedSplineLocation.From(lower)); - - - if (compareToPlane(pos, tan) <= 0) - return computeResult(pos, NormalizedSplineLocation.From(lower)); + if (Math.Abs(oldT.Value - newT.Value) < minStepSize) break; + + if(currentSegment != closest.SegmentIndex) + durationScaling = ops.Div(spline.TotalDuration, closest.Segment.Duration); } - // Edge case: If the queriedPosition is inside the last segment and comes after it, the closest location is totalSegments - if (upper == totalIntervals) { - var (pos, tan) = getPositionAndTangentAtNormalized(NormalizedSplineLocation.From(upper)); - - if (compareToPlane(pos, tan) >= 0) - return computeResult(pos, NormalizedSplineLocation.From(upper)); - } - - var res = NormalizedSplineLocation.From( - lower - + spline[SplineSegmentIndex.At(lower)].Polynomial.EvaluateClosestPointTo(position, samples) - ); - - return computeResult(spline[res].Position, res); + return new ClosestOnSplineQueryResult(position, spline, closest); } } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineQueryResult.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineQueryResult.cs index b1a240f8..0bcdcf31 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineQueryResult.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/ClosestOnSplineQueryResult.cs @@ -4,39 +4,41 @@ namespace BII.WasaBii.Splines { - public readonly struct ClosestOnSplineQueryResult + public readonly struct ClosestOnSplineQueryResult where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged + { internal ClosestOnSplineQueryResult( - TPos queriedPosition, Spline spline, TPos position, NormalizedSplineLocation normalizedLocation + TPos queriedPosition, Spline spline, SplineSample sample ) { QueriedPosition = queriedPosition; Spline = spline; - ClosestOnSpline = position; - NormalizedLocation = normalizedLocation; - cachedLocation = new Lazy(() => spline.DeNormalizeOrThrow(normalizedLocation)); + Sample = sample; + __cachedLocation = new Lazy(() => spline.DeNormalizeOrThrow(sample.NormalizedLocation)); } + + public readonly SplineSample Sample; + + public TTime Time => Sample.GlobalT; /// The normalized location on the spline whose position is closest to the queried position. - public NormalizedSplineLocation NormalizedLocation { get; } + public NormalizedSplineLocation NormalizedLocation => Sample.NormalizedLocation; + + // Since de-normalizing a location may be an expensive operation on long splines, the value is lazy & cached. + private readonly Lazy __cachedLocation; + + /// The location on the spline whose position is closest to the queried position. + public SplineLocation Location => __cachedLocation.Value; /// The position on the spline that is closest to the queried position. - public readonly TPos ClosestOnSpline; + public readonly TPos ClosestOnSpline => Sample.Position; public readonly TPos QueriedPosition; /// The spline where the closest position is on. - public Spline Spline { get; } - - // Since de-normalizing a location may be an expensive operation on long splines, the value is lazy & cached. - private readonly Lazy cachedLocation; - - - /// The location on the spline whose position is closest to the queried position. - public SplineLocation Location => cachedLocation.Value; - - /// The spline's tangent at the location that is closest to the queried position. - public TDiff Tangent => Spline[NormalizedLocation].Tangent; + public Spline Spline { get; } /// The distance between the queried position and the spline / the position . public Length Distance => Spline.Ops.Distance(QueriedPosition, ClosestOnSpline); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/EnumerableClosestOnSplineExtensions.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/EnumerableClosestOnSplineExtensions.cs deleted file mode 100644 index a84386a3..00000000 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/EnumerableClosestOnSplineExtensions.cs +++ /dev/null @@ -1,71 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Diagnostics.Contracts; -using BII.WasaBii.Core; - -namespace BII.WasaBii.Splines { - - public static class EnumerableClosestOnSplineExtensions { - - /// - /// Finds the closest point to a given position on a list of splines. - /// Greedy algorithm which does not always find the global optimum on heavily curved splines. - /// - /// - /// Determines the accuracy of the query. Higher values lead to higher accuracy. - /// However, the default value should be sufficient for all cases. - /// - /// - /// This method should only be used on splines where the distance - /// between pairs of handles is approximately the same. - /// This is because the errorMarginNormalized is relative to the - /// length of a segment between spline handles. - /// Therefore differing distances between handles would lead to different - /// querying accuracies on different points on the spline. - /// - [Pure] public static Option<(TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult)> QueryClosestPositionOnSplinesTo( - this IEnumerable splines, - Func> splineSelector, - TPos position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) where TPos : unmanaged where TDiff : unmanaged => - queryClosestPositionOnSplinesTo( - splines, - queryFunction: withSpline => splineSelector(withSpline).QueryClosestPositionOnSplineTo(position, samples) - ); - - /// - /// Similar to , - /// but a non-al result is returned. - /// Throws when all provided splines are not valid. - /// - /// - /// Determines the accuracy of the query. Higher values lead to higher accuracy. - /// However, the default value should be sufficient for all cases. - /// - /// - /// This method should only be used on splines where the distance - /// between pairs of handles is approximately the same. - /// This is because the errorMarginNormalized is relative to the - /// length of a segment between spline handles. - /// Therefore differing distances between handles would lead to different - /// querying accuracies on different points on the spline. - /// - [Pure] public static (TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult) QueryClosestPositionOnSplinesToOrThrow( - this IEnumerable splines, - Func> splineSelector, - TPos position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - )where TPos : unmanaged where TDiff : unmanaged - => splines.QueryClosestPositionOnSplinesTo(splineSelector, position, samples).GetOrThrow(() => new ArgumentException( - $"All splines given to {nameof(QueryClosestPositionOnSplinesToOrThrow)} were not valid and a query could therefore not be performed!" - )); - - private static Option<(TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult)> queryClosestPositionOnSplinesTo( - IEnumerable splines, - Func>> queryFunction - ) where TPos : unmanaged where TDiff : unmanaged => - splines.Collect(spline => queryFunction(spline).Map(queryResult => (spline, queryResult))) - .MinBy(t => t.queryResult.Distance); - } -} \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/EnumerableClosestOnSplineExtensions.cs.meta b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/EnumerableClosestOnSplineExtensions.cs.meta deleted file mode 100644 index 8393fd69..00000000 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/ClosestOnSplineQueries/EnumerableClosestOnSplineExtensions.cs.meta +++ /dev/null @@ -1,3 +0,0 @@ -fileFormatVersion: 2 -guid: 58837e188aba44cfa46b60c9ba314ee0 -timeCreated: 1569940492 \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/SplineSampleExtensions.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/SplineSampleExtensions.cs index dc53a4dc..9a270ed3 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/SplineSampleExtensions.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Extensions/SplineSampleExtensions.cs @@ -14,13 +14,13 @@ public static class SplineSampleExtensions { /// /// This method samples the positions on the entire spline. /// The sample rate is a defined amount between each segment of spline handles. - /// Returns all sampled positions in order from the begin of the spline to its end. + /// Returns all sampled positions in order from the beginning of the spline to its end. /// The samples are not distributed equidistantly, meaning that the distance between two successive samples /// can vary, especially for splines where the segments vary in length and for some higher-order-segments. /// - [Pure] public static IEnumerable> SampleSplinePerSegment( - this Spline spline, int samplesPerSegment - ) where TPos : unmanaged where TDiff : unmanaged { + [Pure] public static IEnumerable> SampleSplinePerSegment( + this Spline spline, int samplesPerSegment + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { var fromLoc = NormalizedSplineLocation.Zero; var toLoc = NormalizedSplineLocation.From(spline.SegmentCount); @@ -32,24 +32,21 @@ [Pure] public static IEnumerable> SampleSplinePerSegme } /// - /// Behaves similar to + /// Behaves similar to /// but the spline is always sampled along its entire length. /// /// Whether the samples should be uniformly distributed with equal distances between them. /// This prevents samples "clumping together", which can happen especially with higher-order-curves. However, /// it is much more computationally intensive, so leaving this off is significantly faster. - [Pure] public static IEnumerable> SampleSplineEvery( - this Spline spline, + [Pure] public static IEnumerable> SampleSplineEvery( + this Spline spline, Length desiredSampleLength, - int minSamples = 2, - bool equidistant = false - ) where TPos : unmanaged where TDiff : unmanaged - => spline.SampleSplineBetween( + int minSamples = 2 + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.SampleSplineBetween( SplineLocation.Zero, - spline.Length(), + spline.Length, desiredSampleLength, - minSamples, - equidistant + minSamples ); /// @@ -59,14 +56,13 @@ [Pure] public static IEnumerable> SampleSplineEvery Whether the samples should be uniformly distributed with equal distances between them. /// This prevents samples "clumping together", which can happen especially with higher-order-curves. However, /// it is much more computationally intensive, so leaving this off is significantly faster. - [Pure] public static IEnumerable> SampleSpline( - this Spline spline, + [Pure] public static IEnumerable> SampleSpline( + this Spline spline, int samples, bool equidistant = false - ) where TPos : unmanaged where TDiff : unmanaged - => spline.SampleSplineBetween( - SplineLocation.Zero, - spline.Length(), + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.SampleSplineBetween( + NormalizedSplineLocation.Zero, + NormalizedSplineLocation.From(spline.SegmentCount), samples, equidistant ); @@ -79,37 +75,37 @@ [Pure] public static IEnumerable> SampleSpline, /// which can happen especially with higher-order-curves. However, it is much more computationally intensive, /// so turning this off is significantly faster. - [Pure] public static IEnumerable> SampleSplineBetween( - this Spline spline, + [Pure] public static IEnumerable> SampleSplineBetween( + this Spline spline, SplineLocation fromAbsolute, SplineLocation toAbsolute, Length desiredSampleLength, - int minSamples = 2, - bool equidistant = true - ) where TPos : unmanaged where TDiff : unmanaged { + int minSamples = 2 + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { if (desiredSampleLength <= Length.Zero) throw new ArgumentException($"The sampleLength cannot be 0 or smaller than 0 (was {desiredSampleLength})"); var segments = Math.Max(minSamples, (int) Math.Ceiling((toAbsolute - fromAbsolute) / desiredSampleLength) + 1); - return spline.SampleSplineBetween(fromAbsolute, toAbsolute, segments, equidistant); + return spline.SampleSplineBetween(fromAbsolute, toAbsolute, segments, equidistant: true); } /// /// Samples locations on the spline between to . /// - /// uniformly distributed samples /// Whether the samples should be uniformly distributed with equal distances between them. /// This prevents samples "clumping together", which can happen especially with higher-order-curves. However, /// it is much more computationally intensive, so leaving this off is significantly faster. - [Pure] public static IEnumerable> SampleSplineBetween( - this Spline spline, + [Pure] public static IEnumerable> SampleSplineBetween( + this Spline spline, SplineLocation fromAbsolute, SplineLocation toAbsolute, int samples, bool equidistant = false - ) where TPos : unmanaged where TDiff : unmanaged { + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + if(!equidistant) + return spline.SampleSplineBetween(spline.NormalizeOrThrow(fromAbsolute), spline.NormalizeOrThrow(toAbsolute), samples, equidistant: false); var reverse = false; if (toAbsolute < fromAbsolute) { @@ -117,19 +113,34 @@ [Pure] public static IEnumerable> SampleSplineBetween< reverse = true; } - var sampleLocations = equidistant - ? spline.BulkNormalizeOrdered( - SampleRange.From(fromAbsolute, inclusive: true) - .To(toAbsolute, inclusive: true) - .Sample(samples, SplineLocation.Lerp)) - : SampleRange.From(spline.NormalizeOrThrow(fromAbsolute), inclusive: true) - .To(spline.NormalizeOrThrow(toAbsolute), inclusive: true) - .Sample(samples, NormalizedSplineLocation.Lerp); + var sampleLocations = spline.BulkNormalizeOrdered( + SampleRange.From(fromAbsolute, inclusive: true).To(toAbsolute, inclusive: true) + .Sample(samples, SplineLocation.Lerp)); var result = sampleLocations.Select(nl => spline[nl]); return reverse ? result.Reverse() : result; } + /// + /// Samples locations on the spline between to . + /// + /// Whether the samples should be uniformly distributed with equal distances between them. + /// This prevents samples "clumping together", which can happen especially with higher-order-curves. However, + /// it is much more computationally intensive, so leaving this off is significantly faster. + [Pure] public static IEnumerable> SampleSplineBetween( + this Spline spline, + NormalizedSplineLocation from, + NormalizedSplineLocation to, + int samples, + bool equidistant = false + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + if (equidistant) return spline.SampleSplineBetween(spline.DeNormalizeOrThrow(from), spline.DeNormalizeOrThrow(to), samples, equidistant: true); + + return SampleRange.From(from, inclusive: true).To(to, inclusive: true) + .Sample(samples, NormalizedSplineLocation.Lerp) + .Select(nl => spline[nl]); + } + } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/GenericSpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/GenericSpline.cs index 52c33690..e8b3d430 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/GenericSpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/GenericSpline.cs @@ -2,6 +2,7 @@ using System.Collections.Generic; using System.Collections.Immutable; using System.Diagnostics.Contracts; +using System.Linq; using BII.WasaBii.Core; using BII.WasaBii.Splines.Maths; using BII.WasaBii.UnitSystem; @@ -9,49 +10,59 @@ namespace BII.WasaBii.Splines { [MustBeImmutable] - public interface Spline + public interface Spline where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged + { - IEnumerable> Segments { get; } + IEnumerable> Segments { get; } int SegmentCount { get; } - SplineSegment this[SplineSegmentIndex index] { get; } - SplineSample this[SplineLocation location] => this[this.NormalizeOrThrow(location)]; - SplineSample this[NormalizedSplineLocation location] { get; } + Length Length => Segments.Sum(s => s.Length); + TTime TotalDuration => Segments.Aggregate(Ops.ZeroTime, (t, s) => Ops.Add(t, s.Duration)); - ImmutableArray SegmentOffsetsFromBegin { get; } + SplineSample this[TTime t] => + SplineSample.From(this, t); + SplineSegment this[SplineSegmentIndex index] => Segments.Skip(index).First(); + SplineSample this[SplineLocation location] => this[this.NormalizeOrThrow(location)]; + SplineSample this[NormalizedSplineLocation location] => + SplineSample.From(this, location).GetOrThrow(() => + new ArgumentOutOfRangeException( + nameof(location), + location, + $"Must be between 0 and {SegmentCount}" + )); + + ImmutableArray SpatialSegmentOffsets { get; } + ImmutableArray TemporalSegmentOffsets { get; } - GeometricOperations Ops { get; } + GeometricOperations Ops { get; } - [Pure] Spline Map(Func positionMapping, GeometricOperations newOps) - where TPosNew : unmanaged where TDiffNew : unmanaged; - - public interface Copyable : Spline { + [Pure] Spline Map( + Func positionMapping, + GeometricOperations newOps + ) where TPosNew : unmanaged where TDiffNew : unmanaged where TVelNew : unmanaged; - [Pure] public Spline Reversed { get; } + public interface Copyable : Spline { + + [Pure] public Spline Reversed { get; } /// Creates a new spline with a similar trajectory, but with all handle positions - /// being moved by a certain offset which depends on the spline's tangent at these points. - [Pure] public Spline CopyWithOffset(Func tangentToOffset); + /// being moved by a certain offset which depends on the spline's velocity at these points. + [Pure] public Spline CopyWithOffset(Func tangentToOffset); /// Creates a new spline with the same trajectory, but with /// all handle positions being moved along a certain /// , independent of the spline's /// tangent at these points. - [Pure] public Spline CopyWithStaticOffset(TDiff offset); + [Pure] public Spline CopyWithStaticOffset(TDiff offset); /// Creates a new spline with a similar trajectory, /// but different spacing between the handles. - [Pure] public Spline CopyWithDifferentHandleDistance(Length desiredHandleDistance); + [Pure] public Spline CopyWithDifferentHandleDistance(Length desiredHandleDistance); } } - public static class GenericSplineExtensions { - - public static Length Length(this Spline spline) - where TPos : unmanaged where TDiff : unmanaged => spline.Segments.Sum(s => s.Length); - - } - } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/GeometricOperations.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/GeometricOperations.cs index d37c44ef..2f7801a5 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/GeometricOperations.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/GeometricOperations.cs @@ -14,9 +14,11 @@ namespace BII.WasaBii.Splines.Maths { /// implementation of this interface wherever necessary. /// [MustBeImmutable] - public interface GeometricOperations + public interface GeometricOperations where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged + where TVel : unmanaged { [Pure] TPos Add(TPos d1, TDiff d2); [Pure] TDiff Add(TDiff d1, TDiff d2); @@ -28,7 +30,7 @@ public interface GeometricOperations // providing efficient implementations could improve performance [Pure] TPos Sub(TPos p, TDiff d) => Add(p, Mul(d, -1)); [Pure] TDiff Sub(TDiff d1, TDiff d2) => Add(d1, Mul(d2, -1)); - [Pure] TDiff Div(TDiff diff, double d) => Mul(diff, 1 / d); + [Pure] TDiff Div(TDiff diff, double d) => d.IsNearly(1) ? diff : Mul(diff, 1 / d); [Pure] Length Distance(TPos p0, TPos p1) { var diff = Sub(p1, p0); return Math.Sqrt(Dot(diff, diff)).Meters(); @@ -37,26 +39,31 @@ [Pure] Length Distance(TPos p0, TPos p1) { [Pure] TDiff Lerp(TDiff from, TDiff to, double t) => Add(from, Mul(Sub(to, from), t)); [Pure] TDiff ZeroDiff { get; } - } + [Pure] TVel ZeroVel { get; } + [Pure] TTime ZeroTime { get; } - public static class PositionOperationsExtensions { + [Pure] double Div(TTime a, TTime b); - [Pure] - public static TPos Add(this GeometricOperations ops, TPos p, TDiff d1, TDiff d2) - where TPos : unmanaged where TDiff : unmanaged => ops.Add(ops.Add(p, d1), d2); + [Pure] TDiff Mul(TVel v, TTime t); + [Pure] TVel Div(TDiff d, TTime t); + [Pure] TTime Add(TTime a, TTime b); + [Pure] TTime Sub(TTime a, TTime b); + [Pure] TTime Mul(TTime a, double b); - [Pure] - public static TPos Add(this GeometricOperations ops, TPos p, TDiff d1, TDiff d2, TDiff d3) - where TPos : unmanaged where TDiff : unmanaged => ops.Add(ops.Add(p, d1, d2), d3); + } - [Pure] - public static TDiff Add(this GeometricOperations ops, TDiff d1, TDiff d2, TDiff d3) - where TPos : unmanaged where TDiff : unmanaged => ops.Add(ops.Add(d1, d2), d3); + [MustBeImmutable] + public interface ScalarTimeGeometricOperations : GeometricOperations + where TPos : unmanaged where TDiff : unmanaged + { + TDiff GeometricOperations.ZeroVel => ZeroDiff; - [Pure] - public static TDiff Sub(this GeometricOperations ops, TDiff d1, TDiff d2, TDiff d3) - where TPos : unmanaged where TDiff : unmanaged => ops.Sub(ops.Sub(d1, d2), d3); + double GeometricOperations.ZeroTime => 0; + double GeometricOperations.Div(double a, double b) => a / b; + double GeometricOperations.Add(double a, double b) => a + b; + double GeometricOperations.Sub(double a, double b) => a - b; + double GeometricOperations.Mul(double a, double b) => a * b; } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/Polynomial.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/Polynomial.cs index ffc7b33f..e88fe8cf 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/Polynomial.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/Polynomial.cs @@ -11,21 +11,28 @@ namespace BII.WasaBii.Splines.Maths { internal static class Polynomial { - public static Polynomial Quadratic (TPos a, TDiff b, TDiff c, GeometricOperations ops) - where TPos : unmanaged - where TDiff : unmanaged => - new(ops, a, b, c); - - public static Polynomial Cubic (TPos a, TDiff b, TDiff c, TDiff d, GeometricOperations ops) - where TPos : unmanaged - where TDiff : unmanaged => - new(ops, a, b, c, d); + public static Polynomial Quadratic ( + TPos a, TDiff b, TDiff c, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged => + new(ops, duration, a, b, c); + + public static Polynomial Cubic ( + TPos a, TDiff b, TDiff c, TDiff d, + TTime duration, + GeometricOperations ops + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged => + new(ops, duration, a, b, c, d); } - internal readonly struct Polynomial + internal readonly struct Polynomial where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged + where TVel : unmanaged + { /// /// All coefficients of the polynomial function except the first. Eg. a cubic polynomial has @@ -34,24 +41,33 @@ internal readonly struct Polynomial /// private readonly ImmutableArray TailC; private readonly TPos FirstC; + public readonly TTime Duration; - internal readonly GeometricOperations Ops; + internal readonly GeometricOperations Ops; public Length ArcLength => SplineSegmentUtils.SimpsonsLengthOf(this); - public Polynomial(GeometricOperations ops, TPos firstC, ImmutableArray tailC) { + public Polynomial( + GeometricOperations ops, + TTime duration, + TPos firstC, + ImmutableArray tailC + ) { this.FirstC = firstC; this.TailC = tailC; this.Ops = ops; + this.Duration = duration; } - public Polynomial(GeometricOperations ops, TPos firstC, params TDiff[] tailC) { + public Polynomial(GeometricOperations ops, TTime duration, TPos firstC, params TDiff[] tailC) { this.FirstC = firstC; this.TailC = tailC.ToImmutableArray(); + this.Duration = duration; this.Ops = ops; } - public TPos Evaluate(double t) { + public TPos Evaluate(TTime t) => EvaluateNormalized(Ops.Div(t, Duration)); + public TPos EvaluateNormalized(double t) { if(!t.IsInsideInterval(0, 1, threshold: 0.001)) throw new ArgumentException($"The parameter 't' must be between 0 and 1 but it was {t}"); var ops = Ops; return TailC.Aggregate( @@ -60,7 +76,8 @@ public TPos Evaluate(double t) { ).res; } - public TDiff EvaluateDerivative(double t) { + public TVel EvaluateDerivative(TTime t) => Ops.Div(EvaluateDerivativeNormalized(Ops.Div(t, Duration)), Duration); + public TDiff EvaluateDerivativeNormalized(double t) { if(!t.IsInsideInterval(0, 1, threshold: 0.001)) throw new ArgumentException($"The parameter 't' must be between 0 and 1 but it was {t}"); var ops = Ops; return TailC.ZipWithIndices().Aggregate( @@ -75,7 +92,8 @@ public TDiff EvaluateDerivative(double t) { ).res; } - public TDiff EvaluateSecondDerivative(double t) { + public TVel EvaluateSecondDerivative(TTime t) => Ops.Div(EvaluateSecondDerivativeNormalized(Ops.Div(t, Duration)), Duration); + public TDiff EvaluateSecondDerivativeNormalized(double t) { if(!t.IsInsideInterval(0, 1, threshold: 0.001)) throw new ArgumentException($"The parameter 't' must be between 0 and 1 but it was {t}"); var ops = Ops; return TailC.ZipWithIndices().Skip(1).Aggregate( @@ -90,7 +108,8 @@ public TDiff EvaluateSecondDerivative(double t) { ).res; } - public TDiff EvaluateNthDerivative(double t, int n) { + public TVel EvaluateNthDerivative(TTime t, int n) => Ops.Div(EvaluateNthDerivativeNormalized(Ops.Div(t, Duration), n), Duration); + public TDiff EvaluateNthDerivativeNormalized(double t, int n) { if(!t.IsInsideInterval(0, 1, threshold: 0.001)) throw new ArgumentException($"The parameter 't' must be between 0 and 1 but it was {t}"); var ops = Ops; var factorials = new int[TailC.Length + 1]; @@ -119,7 +138,7 @@ double SqrDistanceFactorDerived(double t, TDiff diff, TDiff tan) => ops.Dot(tan, diff); double SqrDistanceFactorTwiceDerived(double t, TDiff diff, TDiff tan) => - ops.Dot(copyOfThis.EvaluateSecondDerivative(t), diff) + ops.Dot(tan, tan); + ops.Dot(copyOfThis.EvaluateSecondDerivativeNormalized(t), diff) + ops.Dot(tan, tan); // We describe the squared distance from the queried position p to the spline as the distance function d(t, qp). // (we use the squared distance, instead of the normal distance, @@ -140,8 +159,8 @@ double SqrDistanceFactorTwiceDerived(double t, TDiff diff, TDiff tan) => // https://en.wikipedia.org/wiki/Newton%27s_method var res = 0.5; for (var i = 0; i < iterations; ++i) { - var pos = copyOfThis.Evaluate(res); - var tan = copyOfThis.EvaluateDerivative(res); + var pos = copyOfThis.EvaluateNormalized(res); + var tan = copyOfThis.EvaluateDerivativeNormalized(res); var diff = ops.Sub(pos, p); var numerator = SqrDistanceFactorDerived(res, diff, tan); var denominator = SqrDistanceFactorTwiceDerived(res, diff, tan); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/SplineNormalizationUtility.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/SplineNormalizationUtility.cs index aa66a394..0c4322bc 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/SplineNormalizationUtility.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Maths/SplineNormalizationUtility.cs @@ -59,15 +59,17 @@ public NormalizedSplineLocationOutOfRangeError(NormalizedSplineLocation location /// Such a conversion is desirable when performance is relevant, /// since operations on are faster. /// - public static Result Normalize( - this Spline spline, + public static Result Normalize( + this Spline spline, SplineLocation location, SplineLocation? splineLocationOvershootTolerance = null ) where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged { - var searchResult = spline.SegmentOffsetsFromBegin.BinarySearch(location); + var searchResult = spline.SpatialSegmentOffsets.BinarySearch(location); var segmentIndex = SplineSegmentIndex.At( searchResult > 0 // location is exactly at intersection of two segments -> result is index of segment that starts here @@ -78,7 +80,7 @@ public static Result No var segment = spline[segmentIndex]; var segmentLength = segment.Length; - var remainingDistanceToLocation = location.Value - spline.SegmentOffsetsFromBegin[segmentIndex]; + var remainingDistanceToLocation = location.Value - spline.SpatialSegmentOffsets[segmentIndex]; var res = NormalizedSplineLocation.From(segmentIndex); return remainingDistanceToLocation switch { @@ -86,20 +88,20 @@ public static Result No res + segment.Polynomial.LengthToProgress(d, cachedPolynomialLength: segmentLength), var d when d.IsNearly(Length.Zero, threshold: splineLocationOvershootTolerance ?? 1E-3.Meters()) => res, var d when d.IsNearly(segmentLength, threshold: splineLocationOvershootTolerance ?? 1E-3.Meters()) => res + 1, - _ => new SplineLocationOutOfRangeError(location, spline.Length()) + _ => new SplineLocationOutOfRangeError(location, spline.Length) }; } - /// + /// /// When the queried /// does not lie on the spline within the , /// i.e. when it is less than or greater than the /// length. - public static NormalizedSplineLocation NormalizeOrThrow( - this Spline spline, + public static NormalizedSplineLocation NormalizeOrThrow( + this Spline spline, SplineLocation location, SplineLocation? splineLocationOvershootTolerance = null - ) where TPos : unmanaged where TDiff : unmanaged => + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.Normalize(location, splineLocationOvershootTolerance).ResultOrThrow(); /// @@ -109,28 +111,30 @@ public static NormalizedSplineLocation NormalizeOrThrow( /// since is equal to the distance /// from the beginning of the spline to the location, in meters. /// - public static Result DeNormalize( - this Spline spline, + public static Result DeNormalize( + this Spline spline, NormalizedSplineLocation t, NormalizedSplineLocation? overshootTolerance = null ) where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged { - if (t < 0) { + if (t.Value < 0) { if (overshootTolerance is { } tolerance && -t <= tolerance) t = NormalizedSplineLocation.Zero; else return new NormalizedSplineLocationOutOfRangeError(t, new(spline.SegmentCount)); } - if (t > spline.SegmentCount) { + if (t.Value > spline.SegmentCount) { if (overshootTolerance is { } tolerance && t - spline.SegmentCount <= tolerance) t = new (spline.SegmentCount); else return new NormalizedSplineLocationOutOfRangeError(t, new(spline.SegmentCount)); } var segmentIndex = new SplineSegmentIndex(Math.Min((int)t.Value, spline.SegmentCount - 1)); - var location = spline.SegmentOffsetsFromBegin[segmentIndex]; + var location = spline.SpatialSegmentOffsets[segmentIndex]; var progressInLastSegment = t.Value - segmentIndex; if (progressInLastSegment > double.Epsilon) { var lastSegment = spline[SplineSegmentIndex.At(segmentIndex)]; @@ -140,16 +144,16 @@ public static Result De return new SplineLocation(location); } - /// + /// /// When the queried /// does not lie on the spline within the , i.e. when it is /// less than or greater than the - /// . - public static SplineLocation DeNormalizeOrThrow( - this Spline spline, + /// . + public static SplineLocation DeNormalizeOrThrow( + this Spline spline, NormalizedSplineLocation t, NormalizedSplineLocation? overshootTolerance = null - ) where TPos : unmanaged where TDiff : unmanaged => + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => spline.DeNormalize(t, overshootTolerance).ResultOrThrow(); /// @@ -161,16 +165,18 @@ public static SplineLocation DeNormalizeOrThrow( /// to avoid situations where points are returned in a different /// order than they were provided, leading to hard-to-understand bugs. /// - /// This method is a more performant alternative to + /// This method is a more performant alternative to /// when normalizing multiple locations at once. /// - public static IEnumerable BulkNormalizeOrdered( - this Spline spline, + public static IEnumerable BulkNormalizeOrdered( + this Spline spline, IEnumerable locations, SplineLocation? splineLocationOvershootTolerance = null ) where TPos : unmanaged - where TDiff : unmanaged { + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged { // Explanation of the algorithm: // To convert from an (absolute) spline location to a normalized spline location, two things are needed: // - The index of the segment the position is in @@ -214,7 +220,7 @@ public static IEnumerable BulkNormalizeOrdered Value = value; [Pure] - public Length GetDistanceToClosestSideOf(Spline spline, Length? cachedLength = null) - where TPos : unmanaged where TDiff : unmanaged { - var length = cachedLength ?? spline.Length(); - var distanceFromEnd = length - Value; + public Length GetDistanceToClosestSideOf(Spline spline) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged { + var distanceFromEnd = spline.Length - Value; return Units.Min(Value, distanceFromEnd); } [Pure] - public bool IsCloserToBeginOf(Spline spline, Length? cachedLength = null) - where TPos : unmanaged where TDiff : unmanaged { - var length = cachedLength ?? spline.Length(); - return (Value < length / 2f); - } + public bool IsCloserToBeginOf(Spline spline) + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + Value < spline.Length / 2f; public static implicit operator Length(SplineLocation l) => l.Value; public static implicit operator double(SplineLocation l) => l.Value.AsMeters(); @@ -121,9 +118,6 @@ [Pure] public static SplineLocation Lerp(SplineLocation from, SplineLocation to, public static NormalizedSplineLocation From(double value) => new(value); public NormalizedSplineLocation(double value) => Value = value; - public static implicit operator double(NormalizedSplineLocation l) => l.Value; - public static explicit operator NormalizedSplineLocation(double l) => new(l); - public static NormalizedSplineLocation operator +(NormalizedSplineLocation l) => l; public static NormalizedSplineLocation operator -(NormalizedSplineLocation l) => diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSample.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSample.cs index b39c0536..b39a7eef 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSample.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSample.cs @@ -1,46 +1,65 @@ +using System; +using System.Collections.Immutable; using System.Diagnostics.Contracts; using BII.WasaBii.Core; using BII.WasaBii.Splines.Maths; namespace BII.WasaBii.Splines { - public readonly struct SplineSample - where TPos : unmanaged - where TDiff : unmanaged { - - public readonly SplineSegment Segment; - - /// The percentage of the sample withing the segment. - public readonly double T; + public readonly struct SplineSample + where TPos : unmanaged + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged { + + public readonly SplineSegment Segment; - public TPos Position => Segment.Polynomial.Evaluate(T); + public readonly TTime GlobalT; + public readonly TTime LocalT; + /// The progress of the sample withing the segment. + public readonly double TNormalized; + public readonly SplineSegmentIndex SegmentIndex; + public NormalizedSplineLocation NormalizedLocation => new(SegmentIndex + TNormalized); + + public TPos Position => Segment.Polynomial.EvaluateNormalized(TNormalized); /// - /// The first derivative, which is the direction of the spline at the queried point. - /// This is also the velocity when traversing the spline at a constant rate. + /// The first derivative, which is the direction of the spline at the queried point multiplied by the current speed. /// - public TDiff Tangent => Segment.Polynomial.EvaluateDerivative(T); + public TVel Velocity => Segment.Polynomial.Ops.Div(DerivativeInSegment, Segment.Duration); + public TDiff DerivativeInSegment => Segment.Polynomial.EvaluateDerivativeNormalized(TNormalized); /// /// The second derivative, which is the direction into which the spline bends at the - /// queried point. This is also the acceleration when traversing the spline at a constant rate. + /// queried point multiplied by the current rate of change in speed. /// - public TDiff Curvature => Segment.Polynomial.EvaluateSecondDerivative(T); + public TVel Acceleration => Segment.Polynomial.Ops.Div(SecondDerivativeInSegment, Segment.Duration); + public TDiff SecondDerivativeInSegment => Segment.Polynomial.EvaluateSecondDerivativeNormalized(TNormalized); - public TDiff NthDerivative(int n) => Segment.Polynomial.EvaluateNthDerivative(T, n); + public TVel NthDerivative(int n) => Segment.Polynomial.Ops.Div(NthDerivativeInSegment(n), Segment.Duration); + public TDiff NthDerivativeInSegment(int n) => Segment.Polynomial.EvaluateNthDerivativeNormalized(TNormalized, n); - public (TPos Position, TDiff Tangent) PositionAndTangent => (Position, Tangent); + public SplineSample(SplineSegment segment, TTime globalT, TTime segmentOffset, SplineSegmentIndex segmentIndex) { + Segment = segment; + GlobalT = globalT; + SegmentIndex = segmentIndex; + LocalT = segment.Polynomial.Ops.Sub(globalT, segmentOffset); + TNormalized = segment.Polynomial.Ops.Div(LocalT, Segment.Duration); + } - public SplineSample(SplineSegment segment, double t) { + public SplineSample(SplineSegment segment, double tNormalized, TTime segmentOffset, SplineSegmentIndex segmentIndex) { Segment = segment; - T = t; + TNormalized = tNormalized; + SegmentIndex = segmentIndex; + LocalT = segment.Polynomial.Ops.Mul(segment.Duration, tNormalized); + GlobalT = segment.Polynomial.Ops.Add(LocalT, segmentOffset); } [Pure] - public static Option> From(Spline spline, SplineLocation location) => + public static Option> From(Spline spline, SplineLocation location) => From(spline, spline.NormalizeOrThrow(location)); [Pure] - public static Option> From(Spline spline, NormalizedSplineLocation location) { + public static Option> From(Spline spline, NormalizedSplineLocation location) { var (segmentIndex, t) = location.AsSegmentIndex(); if (t.IsNearly(0) && segmentIndex.Value == spline.SegmentCount) { segmentIndex -= 1; @@ -48,9 +67,24 @@ public static Option> From(Spline spline, } else if (segmentIndex.Value >= spline.SegmentCount) return Option.None; var segment = spline[segmentIndex]; - return new SplineSample(segment, t); + var minT = spline.TemporalSegmentOffsets[segmentIndex]; + + return new SplineSample(segment, t, minT, segmentIndex); } + [Pure] + public static SplineSample From(Spline spline, TTime time) { + var i = spline.TemporalSegmentOffsets.BinarySearch(time); + if (i < 0) i = ~i - 1; + if (i >= spline.SegmentCount) i = spline.SegmentCount - 1; + if (i < 0) i = 0; + var minT = spline.TemporalSegmentOffsets[i]; + var segmentIndex = new SplineSegmentIndex(i); + var segment = spline[segmentIndex]; + + return new SplineSample(segment, time, minT, segmentIndex); + } + } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSegment.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSegment.cs index b31e135d..9c7963e5 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSegment.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/SplineSegment.cs @@ -6,19 +6,23 @@ using BII.WasaBii.UnitSystem; namespace BII.WasaBii.Splines { - public readonly struct SplineSegment + public readonly struct SplineSegment where TPos : unmanaged - where TDiff : unmanaged { - internal readonly Polynomial Polynomial; + where TDiff : unmanaged + where TTime : unmanaged, IComparable + where TVel : unmanaged + { + internal readonly Polynomial Polynomial; private readonly Lazy cachedLength; public Length Length => cachedLength.Value; - internal SplineSegment(Polynomial polynomial, Lazy? cachedLength = null) { + public readonly TTime Duration; + + internal SplineSegment(Polynomial polynomial, Lazy? cachedLength = null) { Polynomial = polynomial; + Duration = polynomial.Duration; this.cachedLength = cachedLength ?? new Lazy(() => SplineSegmentUtils.SimpsonsLengthOf(polynomial)); } - - public SplineSample SampleAt(double percentage) => new(this, percentage); } public static class SplineSegmentUtils { @@ -28,15 +32,15 @@ public static class SplineSegmentUtils { /// applying the trapezoidal rule with sections. /// [Pure] - internal static Length TrapezoidalLengthOf( - Polynomial polynomial, + internal static Length TrapezoidalLengthOf( + Polynomial polynomial, double? start = 0.0, double? end = 1.0, int samples = 10 - ) where TPos : unmanaged where TDiff : unmanaged { + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged { var range = SampleRange.From(start ?? 0.0, inclusive: true).To(end ?? 1.0, inclusive: true); var ops = polynomial.Ops; return range.Sample(samples + 1, (a, b, p) => MathD.Lerp(a, b, p)) - .Select(polynomial.Evaluate) + .Select(polynomial.EvaluateNormalized) .PairwiseSliding() .Sum(sample => ops.Distance(sample.Item1, sample.Item2)); } @@ -46,16 +50,16 @@ internal static Length TrapezoidalLengthOf( /// Simpson's 1/3 rule with sections / double that in subsections. /// [Pure] - internal static Length SimpsonsLengthOf( - Polynomial polynomial, + internal static Length SimpsonsLengthOf( + Polynomial polynomial, double? start = 0.0, double? end = 1.0, int sections = 4 - ) where TPos : unmanaged where TDiff : unmanaged { + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged { // The function whose integral in the (0..1) range gives the polynomial curve's length. // This is what we want to approximate. double LengthDeriv(double t) { - var v = polynomial.EvaluateDerivative(t); + var v = polynomial.EvaluateDerivativeNormalized(t); var ret = Math.Sqrt(polynomial.Ops.Dot(v, v)); return ret; } @@ -68,11 +72,11 @@ double LengthDeriv(double t) { /// Approximates the polynomial curve length between 0 and . /// [Pure] - internal static Length ProgressToLength( - this Polynomial polynomial, double t, + internal static Length ProgressToLength( + this Polynomial polynomial, double t, int approximationSampleSectionCount = 4 - ) where TPos : unmanaged where TDiff : unmanaged - => SimpsonsLengthOf(polynomial, end: t, sections: approximationSampleSectionCount); + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged => + SimpsonsLengthOf(polynomial, end: t, sections: approximationSampleSectionCount); /// /// Iteratively approximates the progress parameter t where the length of the @@ -92,17 +96,17 @@ internal static Length ProgressToLength( /// Triggers an early return when the current curve segment is within a factor /// of from the queried /// The total polynomial arc length if already known - /// How many sections to sample when calculating the + /// How many sections to sample when calculating the [Pure] - internal static double LengthToProgress( - this Polynomial polynomial, + internal static double LengthToProgress( + this Polynomial polynomial, Length length, int iterations = 2, double oversteppingFactor = 1.1, double thresholdFactor = 1.01, Length? cachedPolynomialLength = null, int approximationSampleSectionCount = 2 - ) where TPos : unmanaged where TDiff : unmanaged { + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged where TVel : unmanaged { if (thresholdFactor < 1) throw new ArgumentException($"{nameof(thresholdFactor)} must be at least 1, was {thresholdFactor}"); @@ -110,6 +114,9 @@ internal static double LengthToProgress( var lowerThreshold = 1 / thresholdFactor; var totalLength = cachedPolynomialLength ?? SimpsonsLengthOf(polynomial, sections: approximationSampleSectionCount); + + if (totalLength.IsNearly(Length.Zero)) + return 0; // Degenerate / zero-length segment. Progress does not matter since the position and velocity are the same everywhere. var lowerBound = (t: 0.0, length: Length.Zero); var upperBound = (t: 1.0, length: totalLength); @@ -147,11 +154,10 @@ internal static double LengthToProgress( } [Pure] - public static NormalizedSplineLocation ClosestPointInSegmentTo( - this SplineSample sample, TPos queriedPosition, int samples - ) - where TPos : unmanaged - where TDiff : unmanaged => NormalizedSplineLocation.From(sample.T + sample.Segment.Polynomial.EvaluateClosestPointTo(queriedPosition, samples)); + public static NormalizedSplineLocation ClosestPointInSegmentTo( + this SplineSample sample, TPos queriedPosition, int samples + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + NormalizedSplineLocation.From(sample.TNormalized + sample.Segment.Polynomial.EvaluateClosestPointTo(queriedPosition, samples)); } } \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmulRomSplineTests.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmulRomSplineTests.cs index ca54561f..f0a41b09 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmulRomSplineTests.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmulRomSplineTests.cs @@ -12,9 +12,9 @@ public void Ctor_WhenInitializedCorrectly_ThenCorrectNodePositionsAndValidSpline var lastHandle = new Vector3(3, 0, 0); var endMarginHandle = new Vector3(4, 0, 0); - var uut = CatmullRomSpline.FromHandlesIncludingMarginOrThrow( + var uut = CatmullRomSpline.UniformFromHandlesIncludingMarginOrThrow( new[] { beginMarginHandle, firstHandle, lastHandle, endMarginHandle }, - UnitySpline.GeometricOperations.Instance + UniformUnitySpline.GeometricOperations.Instance ); Assert.That(uut[SplineHandleIndex.At(0)], Is.EqualTo(beginMarginHandle)); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmullRomSegmentTest.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmullRomSegmentTest.cs index c14d3b1e..6d529ee0 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmullRomSegmentTest.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/CatmullRomSegmentTest.cs @@ -7,13 +7,13 @@ namespace BII.WasaBii.Splines.Tests { public class CatmullRomSegmentTest { - private void assertExistsAndEquals( - CatmullRomSegment? segment, + private void assertExistsAndEquals( + CatmullRomSegment? segment, Vector3 expectedP0, Vector3 expectedP1, Vector3 expectedP2, Vector3 expectedP3 - ) { + ) where TTime : unmanaged where TVel : unmanaged { Assert.That(segment.HasValue); if (segment is { } val) { Assert.That(val.P0, Is.EqualTo(expectedP0)); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/ClosestOnSplineTests.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/ClosestOnSplineTests.cs index e1384b40..2e57c480 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/ClosestOnSplineTests.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/ClosestOnSplineTests.cs @@ -8,7 +8,7 @@ public class ClosestOnSplineTests { [Test] public void QueryGreedyClosestPositionOnSplineTo_WhenEquidistantNodes_ThenReturnsCorrectLocationAndDistance() { - var uut = UnitySpline.FromHandlesIncludingMargin(new []{ + var uut = UniformUnitySpline.FromHandlesIncludingMargin(new []{ new Vector3(-1, 0, 0), new Vector3(0, 0, 0), new Vector3(1, 0, 0), @@ -20,7 +20,7 @@ public void QueryGreedyClosestPositionOnSplineTo_WhenEquidistantNodes_ThenReturn for (var xCoord = -2f; xCoord < 5; xCoord += 0.1f) { var position = new Vector3(xCoord, -1, 0); var queryResult = - uut.QueryClosestPositionOnSplineToOrThrow(position); + uut.QueryClosestPositionOnSplineTo(position); var expectedLocationOnSpline = Mathf.Clamp(xCoord, 0, 3); var expectedPositionOnSpline = new Vector3(Mathf.Clamp(xCoord, 0, 3), 0, 0); var expectedPositionToNodeDistance = (double) Vector3.Distance( diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/EnumerableToSplineExtensionsTests.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/EnumerableToSplineExtensionsTests.cs index cf2e73b5..bd6b3ad3 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/EnumerableToSplineExtensionsTests.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/EnumerableToSplineExtensionsTests.cs @@ -11,7 +11,7 @@ public class EnumerableToSplineExtensionsTests { public void ToSplineOrThrow_WhenLessThanTwoNodes_ThenThrowsInsufficientNodePositionsException() { var positions = new[] { Vector3.zero }; - Assert.That(() => UnitySpline.FromHandles(positions), Throws.TypeOf()); + Assert.That(() => UniformUnitySpline.FromHandles(positions), Throws.TypeOf()); } [Test] @@ -23,7 +23,7 @@ public void ToSplineOrThrow_WhenTwoNodes_ThenReturnsCorrectSplineWithCorrectHand var expectedBeginHandle = new Vector3(-3, 0, 0); var expectedEndHandle = new Vector3(3, 0, 0); - var uut = UnitySpline.FromHandles(positions).AsOrThrow>(); + var uut = UniformUnitySpline.FromHandles(positions).AsOrThrow>(); Assert.That(uut.BeginMarginHandle(), Is.EqualTo(expectedBeginHandle)); Assert.That(uut.FirstHandle(), Is.EqualTo(first)); Assert.That(uut.LastHandle(), Is.EqualTo(last)); @@ -34,7 +34,7 @@ public void ToSplineOrThrow_WhenTwoNodes_ThenReturnsCorrectSplineWithCorrectHand public void ToSplineOrNone_WhenLessThanTwoNodes_ThenReturnsNull() { var positions = new[] { Vector3.zero }; - var uut = CatmullRomSpline.FromHandles(positions, UnitySpline.GeometricOperations.Instance); + var uut = CatmullRomSpline.UniformFromHandles(positions, UniformUnitySpline.GeometricOperations.Instance); Assert.AreEqual(uut, new CatmullRomSpline.NotEnoughHandles(1, 2).Failure()); } @@ -48,7 +48,7 @@ public void ToSplineOrNone_WhenTwoNodes_ThenReturnsCorrectSplineWithCorrectHandl var expectedBeginHandle = new Vector3(-3, 0, 0); var expectedEndHandle = new Vector3(3, 0, 0); - var uutO = CatmullRomSpline.FromHandles(positions, UnitySpline.GeometricOperations.Instance); + var uutO = CatmullRomSpline.UniformFromHandles(positions, UniformUnitySpline.GeometricOperations.Instance); Assert.AreEqual(uutO.WasFailure, false); var uut = uutO.ResultOrThrow(); Assert.That(uut.BeginMarginHandle(), Is.EqualTo(expectedBeginHandle)); @@ -61,7 +61,7 @@ public void ToSplineOrNone_WhenTwoNodes_ThenReturnsCorrectSplineWithCorrectHandl public void ToSplineWithHandles_WhenLessThanFourNodes_ThenThrowsInsufficientNodePositionsException() { var positions = new[] { Vector3.zero, Vector3.one, Vector3.one }; - Assert.That(() => UnitySpline.FromHandlesIncludingMargin(positions), Throws.TypeOf()); + Assert.That(() => UniformUnitySpline.FromHandlesIncludingMargin(positions), Throws.TypeOf()); } [Test] @@ -72,7 +72,7 @@ public void ToSpline_WhenFourNodes_ThenReturnsCorrectSpline() { var endHandle = new Vector3(3, 0, 0); var positions = new[] { beginHandle, first, last, endHandle }; - var uut = UnitySpline.FromHandlesIncludingMargin(positions).AsOrThrow>(); + var uut = UniformUnitySpline.FromHandlesIncludingMargin(positions).AsOrThrow>(); Assert.That(uut.BeginMarginHandle(), Is.EqualTo(beginHandle)); Assert.That(uut.FirstHandle(), Is.EqualTo(first)); Assert.That(uut.LastHandle(), Is.EqualTo(last)); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/PolynomialTests.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/PolynomialTests.cs index 6fe483dc..ae97cf8b 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/PolynomialTests.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/PolynomialTests.cs @@ -4,7 +4,6 @@ using BII.WasaBii.Geometry; using BII.WasaBii.Splines.Maths; using BII.WasaBii.UnitSystem; -using BII.WasaBii.Unity.Geometry; using NUnit.Framework; using UnityEngine; @@ -19,17 +18,19 @@ public class PolynomialTests { private static readonly GlobalOffset e = new(-1.0f / 12.0f, 2 * Mathf.PI, (float)Math.E); private static readonly GlobalOffset f = new(1.618f, 2.414f, 3.303f); - private static readonly Polynomial linearPolynomial = new( - GlobalSpline.GeometricOperations.Instance, + private static readonly Polynomial linearPolynomial = new( + UniformGlobalSpline.GeometricOperations.Instance, + 1, a, b ); - private static readonly Polynomial cubicPolynomial = Polynomial.Cubic( - a, b, c, d, GlobalSpline.GeometricOperations.Instance + private static readonly Polynomial cubicPolynomial = Polynomial.Cubic( + a, b, c, d, 1, UniformGlobalSpline.GeometricOperations.Instance ); - private static readonly Polynomial sixthOrderPolynomial = new( - GlobalSpline.GeometricOperations.Instance, + private static readonly Polynomial sixthOrderPolynomial = new( + UniformGlobalSpline.GeometricOperations.Instance, + 1, a, b, c, d, e, f ); @@ -173,12 +174,16 @@ public void EvaluateSixth_SixthDerivative() { // that its anti-derivative is simply the polynomial evaluation (ignoring the constant bias, which cancels out). // Hence, we only test against those, as we can validate the result there. - private sealed class OneDimensionalOps : GeometricOperations { + private sealed class OneDimensionalOps : GeometricOperations { public double Add(double a, double b) => a + b; public double Sub(double a, double b) => a - b; public double Dot(double a, double b) => a * b; public double Mul(double a, double b) => a * b; public double ZeroDiff => 0; + public double ZeroTime => 0; + public double ZeroVel => 0; + public double InverseLerp(double min, double max, double value) => (value - min) / (max - min); + public double Div(double d, double t) => d / t; } private const int normalizationTestSampleCount = 20; @@ -187,7 +192,7 @@ private sealed class OneDimensionalOps : GeometricOperations { public void DeNormalizeMonotoneRisingCubic1DSplineLocations() { // Positive parameters ensure a positive derivative (for positive t values) and thus a monotone rising polynomial. - var oneDimensionalPolynomial = Polynomial.Cubic(1, 3, 3, 7, new OneDimensionalOps()); + var oneDimensionalPolynomial = Polynomial.Cubic(1, 3, 3, 7, duration: 1, new OneDimensionalOps()); foreach (var t in SampleRange.Sample01(normalizationTestSampleCount, includeZero: true, includeOne: true)) { var expected = oneDimensionalPolynomial.Evaluate(t) - oneDimensionalPolynomial.Evaluate(0); @@ -203,7 +208,7 @@ public void NormalizeMonotoneRisingQuadratic1DSplineLocations() { const int p0 = 2; const int p1 = 8; const int p2 = 8; - var polynomial = new Polynomial(new OneDimensionalOps(), p0, p1, p2); + var polynomial = new Polynomial(new OneDimensionalOps(), duration: 1, p0, p1, p2); // length(t) is p1*t + p2*t² // thus, 0 == p2*t² + p1*t - length diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineNormalizationUtilityTest.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineNormalizationUtilityTest.cs index 13c6d22e..fffdc2b1 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineNormalizationUtilityTest.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineNormalizationUtilityTest.cs @@ -7,13 +7,13 @@ namespace BII.WasaBii.Splines.Tests { public class SplineNormalizationUtilityTest { - private static readonly Dictionary normalizaionSamples = new Dictionary + private static readonly Dictionary normalizationSamples = new Dictionary {{0, 0}, {0.5, 0.202}, {1, 0.322}, {2.6, 0.622}, {3, 0.696}, {4.414, 1}}.ToDictionary( kvp => SplineLocation.From(kvp.Key), kvp => NormalizedSplineLocation.From(kvp.Value) ); - private static readonly Dictionary deNormalizaionSamples = new Dictionary + private static readonly Dictionary deNormalizationSamples = new Dictionary {{0, 0}, {0.1, 0.192}, {0.3, 0.898}, {0.55, 2.198}, {0.7, 3.023}, {1, 4.414}}.ToDictionary( kvp => NormalizedSplineLocation.From(kvp.Key), kvp => SplineLocation.From(kvp.Value) @@ -23,7 +23,7 @@ public class SplineNormalizationUtilityTest { public void DeNormalize_BatchTest() { var uut = SplineTestUtils.ExampleCurvedSpline.Spline; - foreach (var kvp in deNormalizaionSamples) { + foreach (var kvp in deNormalizationSamples) { var location = uut.DeNormalizeOrThrow(kvp.Key); Assert.That(location.Value.SiValue, Is.EqualTo(kvp.Value.Value.SiValue).Within(SplineLocationTolerance)); } @@ -45,7 +45,7 @@ public void DeNormalize_WhenEquidistantNode_ThenTAndLocationEqual() { public void Normalize_BatchTest() { var uut = SplineTestUtils.ExampleCurvedSpline.Spline; - foreach (var kvp in normalizaionSamples) { + foreach (var kvp in normalizationSamples) { var t = uut.NormalizeOrThrow(kvp.Key); Assert.That(t.Value, Is.EqualTo(kvp.Value.Value).Within(SplineLocationTolerance)); } @@ -70,7 +70,7 @@ public void Normalize_WhenSegmentLengthAsLocation_ThenIntegerValueReturned() { var uut = spline.NormalizeOrThrow(length); - Assert.That(uut.Value, Is.EqualTo((int) uut)); + Assert.That(uut.Value, Is.EqualTo((int) uut.Value)); } @@ -78,11 +78,11 @@ public void Normalize_WhenSegmentLengthAsLocation_ThenIntegerValueReturned() { public void BulkNormalizeOrdered_BatchTest() { var uut = SplineTestUtils.ExampleCurvedSpline.Spline; - var toNormalize = new SplineLocation[normalizaionSamples.Count]; - var expected = new NormalizedSplineLocation[deNormalizaionSamples.Count]; + var toNormalize = new SplineLocation[normalizationSamples.Count]; + var expected = new NormalizedSplineLocation[deNormalizationSamples.Count]; int index = 0; - foreach(var kvp in normalizaionSamples) { + foreach(var kvp in normalizationSamples) { toNormalize[index] = kvp.Key; expected[index] = kvp.Value; } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineTestUtils.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineTestUtils.cs index ddd5e34f..9e4878fc 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineTestUtils.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Tests/CatmullRom/SplineTestUtils.cs @@ -7,7 +7,7 @@ namespace BII.WasaBii.Splines.Tests { - using Spline = CatmullRomSpline; + using Spline = CatmullRomSpline; internal class SplineTestUtils { @@ -46,7 +46,7 @@ public static class ExampleInvalidSpline { public static Vector3 SecondHandle = new Vector3(0, 0, 2); public const int HandleCount = 2; - public static Spline Spline => UnitySpline.FromHandlesIncludingMargin( + public static Spline Spline => UniformUnitySpline.FromHandlesIncludingMargin( new []{FirstHandle, SecondHandle}, SplineType.Centripetal ).AsOrThrow(); @@ -72,11 +72,11 @@ public static class ExampleLinearSpline { public static Vector3 Expected05Curvature = Vector3.zero; public static Vector3 Expected1Curvature = Vector3.zero; - public static Polynomial Polynomial => - new CatmullRomSegment(FirstHandle, SecondHandle, ThirdHandle, FourthHandle, UnitySpline.GeometricOperations.Instance) + public static Polynomial Polynomial => + new CatmullRomSegment(FirstHandle, SecondHandle, ThirdHandle, FourthHandle, 1, 1, 1, UniformUnitySpline.GeometricOperations.Instance) .ToPolynomial(splineTypeAlphaValue); - public static Spline Spline => UnitySpline.FromHandlesIncludingMargin( + public static Spline Spline => UniformUnitySpline.FromHandlesIncludingMargin( new[]{FirstHandle, SecondHandle, ThirdHandle, FourthHandle}, SplineType.Centripetal ).AsOrThrow(); @@ -91,7 +91,7 @@ public static class ExampleEquidistantLinearSpline { public static Vector3 FourthHandle = new Vector3(0, 0, 4); public static Vector3 FifthHandle = new Vector3(0, 0, 5); - public static Spline Spline => UnitySpline.FromHandlesIncludingMargin( + public static Spline Spline => UniformUnitySpline.FromHandlesIncludingMargin( new[]{FirstHandle, SecondHandle, ThirdHandle, FourthHandle, FifthHandle}, SplineType.Centripetal ).AsOrThrow(); @@ -121,11 +121,11 @@ public static class ExampleCurvedSpline { public static Length ExpectedSplineLength => 4.413755.Meters(); - public static Polynomial Polynomial => - new CatmullRomSegment(FirstHandle, SecondHandle, ThirdHandle, FourthHandle, UnitySpline.GeometricOperations.Instance) + public static Polynomial Polynomial => + new CatmullRomSegment(FirstHandle, SecondHandle, ThirdHandle, FourthHandle, 1, 1, 1, UniformUnitySpline.GeometricOperations.Instance) .ToPolynomial(splineTypeAlphaValue); - public static Spline Spline => UnitySpline.FromHandlesIncludingMargin( + public static Spline Spline => UniformUnitySpline.FromHandlesIncludingMargin( new[]{FirstHandle, SecondHandle, ThirdHandle, FourthHandle}, SplineType.Centripetal ).AsOrThrow(); diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/PartialSpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/PartialSpline.cs index 64221211..9e8a61b0 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/PartialSpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/PartialSpline.cs @@ -1,5 +1,4 @@ using System; -using System.Diagnostics; using BII.WasaBii.Core; using BII.WasaBii.Splines.Maths; using BII.WasaBii.UnitSystem; @@ -16,15 +15,16 @@ public enum SampleDirection { /// to . /// [MustBeImmutable][Serializable] - public readonly struct PartialSpline where TPos : unmanaged where TDiff : unmanaged { - public readonly Spline Spline; + public readonly struct PartialSpline where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged + { + public readonly Spline Spline; public readonly SplineLocation StartLocation; public readonly SplineLocation EndLocation; public readonly NormalizedSplineLocation StartLocationNormalized; public readonly NormalizedSplineLocation EndLocationNormalized; public readonly Length Length; - public PartialSpline(Spline spline, SplineLocation startLocation, SplineLocation endLocation) { + public PartialSpline(Spline spline, SplineLocation startLocation, SplineLocation endLocation) { Spline = spline; StartLocation = startLocation; EndLocation = endLocation; @@ -35,9 +35,9 @@ public PartialSpline(Spline spline, SplineLocation startLocation, S if(Length < Length.Zero) throw new ArgumentException($"PartialSpline must have a positive length (was {Length})"); } - public SplineSample SampleAt(double percentage) => Spline[NormalizedSplineLocation.Lerp(StartLocationNormalized, EndLocationNormalized, percentage)]; + public SplineSample SampleAt(double percentage) => Spline[NormalizedSplineLocation.Lerp(StartLocationNormalized, EndLocationNormalized, percentage)]; - public SplineSample SampleFromStart(Length distanceFromStart) { + public SplineSample SampleFromStart(Length distanceFromStart) { if(distanceFromStart < -Length.Epsilon) throw new ArgumentException( $"Distance must be above 0, but was {distanceFromStart}" ); @@ -47,7 +47,7 @@ public SplineSample SampleFromStart(Length distanceFromStart) { return Spline[distanceFromStart + StartLocation]; } - public SplineSample SampleFromEnd(Length distanceFromEnd) { + public SplineSample SampleFromEnd(Length distanceFromEnd) { if(distanceFromEnd < -Length.Epsilon) throw new ArgumentException( $"Distance must be above 0, but was {distanceFromEnd}" ); @@ -57,7 +57,7 @@ public SplineSample SampleFromEnd(Length distanceFromEnd) { return Spline[EndLocation - distanceFromEnd]; } - public SplineSample SampleFrom(SampleDirection direction, Length distance) => direction switch { + public SplineSample SampleFrom(SampleDirection direction, Length distance) => direction switch { SampleDirection.FromStart => SampleFromStart(distance), SampleDirection.FromEnd => SampleFromEnd(distance), _ => throw new UnsupportedEnumValueException(direction) diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SpecificSplineBase.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SpecificSplineBase.cs index 728a40c5..516af572 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SpecificSplineBase.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SpecificSplineBase.cs @@ -3,8 +3,6 @@ using System.Collections.Immutable; using System.Diagnostics.Contracts; using BII.WasaBii.Core; -using BII.WasaBii.Splines.Bezier; -using BII.WasaBii.Splines.CatmullRom; using BII.WasaBii.Splines.Maths; using BII.WasaBii.UnitSystem; @@ -12,101 +10,103 @@ namespace BII.WasaBii.Splines { /// /// Base class for non-generic spline implementations like that hides - /// the type of a low-level spline like - /// or by wrapping it. + /// the type of a low-level spline like + /// or by wrapping it. /// [Serializable] - public abstract class SpecificSplineBase : Spline.Copyable - where TSelf : SpecificSplineBase where TPos : unmanaged where TDiff : unmanaged { + public abstract class SpecificSplineBase : Spline.Copyable + where TSelf : SpecificSplineBase + where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged + { - public readonly Spline Wrapped; + public readonly Spline Wrapped; - protected SpecificSplineBase(Spline wrapped) => Wrapped = wrapped switch { - SpecificSplineBase {Wrapped: var actualWrapped} => actualWrapped, + protected SpecificSplineBase(Spline wrapped) => Wrapped = wrapped switch { + SpecificSplineBase {Wrapped: var actualWrapped} => actualWrapped, _ => wrapped }; - [Pure] public Option As() where T : Spline => Wrapped switch { + [Pure] public Option As() where T : Spline => Wrapped switch { T t => t.Some(), _ => Option.None }; - [Pure] public T AsOrThrow() where T : Spline => Wrapped is T t + [Pure] public T AsOrThrow() where T : Spline => Wrapped is T t ? t : throw new InvalidCastException($"{Wrapped.GetType().Name} is no {nameof(T)}"); - public int SegmentCount => Wrapped.SegmentCount; - public IEnumerable> Segments => Wrapped.Segments; - - public SplineSegment this[SplineSegmentIndex index] => Wrapped[index]; + public Length Length => Wrapped.Length; + public TTime TotalDuration => Wrapped.TotalDuration; - public SplineSample this[NormalizedSplineLocation location] => Wrapped[location]; + public int SegmentCount => Wrapped.SegmentCount; + public IEnumerable> Segments => Wrapped.Segments; - public ImmutableArray SegmentOffsetsFromBegin => Wrapped.SegmentOffsetsFromBegin; - public GeometricOperations Ops => Wrapped.Ops; + public SplineSample this[TTime t] => Wrapped[t]; + public SplineSegment this[SplineSegmentIndex index] => Wrapped[index]; + public SplineSample this[SplineLocation location] => Wrapped[location]; + public SplineSample this[NormalizedSplineLocation location] => Wrapped[location]; - [Pure] public Spline Map( - Func positionMapping, - GeometricOperations newOps - ) where TPosNew : unmanaged where TDiffNew : unmanaged => Wrapped.Map(positionMapping, newOps); + public ImmutableArray SpatialSegmentOffsets => Wrapped.SpatialSegmentOffsets; + public ImmutableArray TemporalSegmentOffsets => Wrapped.TemporalSegmentOffsets; + public GeometricOperations Ops => Wrapped.Ops; - /// - [Pure] - public ClosestOnSplineQueryResult QueryClosestPositionOnSplineToOrThrow( - TPos position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => this.QueryClosestPositionOnSplineToOrThrow(position, samples); + [Pure] public Spline Map( + Func positionMapping, GeometricOperations newOps + ) where TPosNew : unmanaged where TDiffNew : unmanaged where TVelNew : unmanaged => + Wrapped.Map(positionMapping, newOps); - /// + /// [Pure] - public Option> QueryClosestPositionOnSplineTo( + public ClosestOnSplineQueryResult QueryClosestPositionOnSplineTo( TPos position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => this.QueryClosestPositionOnSplineTo(position, samples); + int initialSamples = ClosestOnSplineExtensions.DefaultInitialSamplingCount, + int iterations = ClosestOnSplineExtensions.DefaultIterations, + double minStepSize = ClosestOnSplineExtensions.DefaultMinStepSize + ) => this.QueryClosestPositionOnSplineTo(position, initialSamples, iterations, minStepSize); - /// + /// [Pure] - public TSelf CopyWithOffset(Func tangentToOffset) => - mkNew(((Spline.Copyable)this).CopyWithOffset(tangentToOffset)); + public TSelf CopyWithOffset(Func tangentToOffset) => + mkNew(((Spline.Copyable)this).CopyWithOffset(tangentToOffset)); - Spline Spline.Copyable.CopyWithOffset(Func tangentToOffset) => + Spline Spline.Copyable.CopyWithOffset(Func tangentToOffset) => Wrapped switch { - Spline.Copyable copyable => copyable.CopyWithOffset(tangentToOffset), - _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") + Spline.Copyable copyable => copyable.CopyWithOffset(tangentToOffset), + _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") }; - /// + /// [Pure] public TSelf CopyWithStaticOffset(TDiff offset) => - mkNew(((Spline.Copyable)this).CopyWithStaticOffset(offset)); + mkNew(((Spline.Copyable)this).CopyWithStaticOffset(offset)); - Spline Spline.Copyable.CopyWithStaticOffset(TDiff offset) => + Spline Spline.Copyable.CopyWithStaticOffset(TDiff offset) => Wrapped switch { - Spline.Copyable copyable => copyable.CopyWithStaticOffset(offset), - _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") + Spline.Copyable copyable => copyable.CopyWithStaticOffset(offset), + _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") }; - /// + /// [Pure] public TSelf CopyWithDifferentHandleDistance(Length desiredHandleDistance) => - mkNew(((Spline.Copyable)this).CopyWithDifferentHandleDistance(desiredHandleDistance)); + mkNew(((Spline.Copyable)this).CopyWithDifferentHandleDistance(desiredHandleDistance)); - Spline Spline.Copyable.CopyWithDifferentHandleDistance(Length desiredHandleDistance) => + Spline Spline.Copyable.CopyWithDifferentHandleDistance(Length desiredHandleDistance) => Wrapped switch { - Spline.Copyable copyable => copyable.CopyWithDifferentHandleDistance(desiredHandleDistance), - _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") + Spline.Copyable copyable => copyable.CopyWithDifferentHandleDistance(desiredHandleDistance), + _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") }; - /// - public TSelf Reversed => mkNew(((Spline.Copyable)this).Reversed); + /// + public TSelf Reversed => mkNew(((Spline.Copyable)this).Reversed); - Spline Spline.Copyable.Reversed => + Spline Spline.Copyable.Reversed => Wrapped switch { - Spline.Copyable copyable => copyable.Reversed, - _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") + Spline.Copyable copyable => copyable.Reversed, + _ => throw new Exception($"Spline type {Wrapped.GetType()} does not implement {nameof(Spline.Copyable)}") }; - [Pure] protected abstract TSelf mkNew(Spline toWrap); + [Pure] protected abstract TSelf mkNew(Spline toWrap); } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SplineUtils.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SplineUtils.cs index 8ffdec60..1e563ae8 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SplineUtils.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/Utils/SplineUtils.cs @@ -1,4 +1,5 @@ -using System.Diagnostics.Contracts; +using System; +using System.Diagnostics.Contracts; using BII.WasaBii.Core; namespace BII.WasaBii.Splines { @@ -6,14 +7,16 @@ namespace BII.WasaBii.Splines { public static class SplineUtils { [Pure] - public static Option> TryQuery( - this Spline spline, NormalizedSplineLocation location - ) where TPos : unmanaged where TDiff : unmanaged => SplineSample.From(spline, location); + public static Option> TryQuery( + this Spline spline, NormalizedSplineLocation location + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + SplineSample.From(spline, location); [Pure] - public static Option> TryQuery( - this Spline spline, SplineLocation location - ) where TPos : unmanaged where TDiff : unmanaged => SplineSample.From(spline, location); + public static Option> TryQuery( + this Spline spline, SplineLocation location + ) where TPos : unmanaged where TDiff : unmanaged where TTime : unmanaged, IComparable where TVel : unmanaged => + SplineSample.From(spline, location); } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/_docs.md b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/_docs.md index 91f9e0b1..073690a2 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/_docs.md +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Splines/_docs.md @@ -2,23 +2,23 @@ ## Overview -This module provides an implementation of Catmull-Rom splines and high-level wrappers around them. +This module provides an implementation of Catmull-Rom and Bezier splines and high-level wrappers around them. Splines are implicit mathematical representations of "curved lines" and can be used for Animations, Movement and curved Objects. ## Spline Fundamentals -These splines are defined by a collection of points, called handles. -All handles but the first and last handles are used to interpolate the curve. -The first and last handles are called *margin handles*, and the other ones will be referred to as *interpolated handles*. -*Margin handles* do not contribute to the interpolation, but determine the curvature and the beginning and end of the spline. +A spline is a collection of spline segment, where each segment is a curved line between two points. A segment's trajectory is usually influenced by additional points, depending on the spline type. +The collection of a spline's points is called its *handles*. +Additionally, spline segments each have a duration, describing how long it takes to traverse them. Thus, the spline itself has a duration, which is simply the sum of its segments'. -- The `WithSpline` interface is implemented by any class that has an underlying spline, but that is not a spline itself. -- A `Spline` is the implementation of a Catmull-Rom-Spline with a list of handles. A valid spline must have at least 4 handles (2 margin, 2 interpolated). -- A `SplineSegment` is the segment between two interpolated handles. There are always exactly `handleCount - 3` of them. -- A `SplineSample` is a sampled location in between a segment. Whereas a segment is a curved line, the sample is a location on that line. +- A `Spline` is the base interface for all spline types. Built-in implementations are the `CatmullRomSpline` and the `BezierSpline`. +- A `SplineSegment` is the segment between two interpolated handles. +- A `SplineSample` is a sampled location somewhere on a spline. It provides information about the position, velocity / tangent and acceleration / curvature of the spline (segment) at that location. ### Generics +#### Handles + Splines are generic over their handle type. This means that you are not forced to build them using System.Numerics or Unity `Vector3`s, but any type you want. For example, you could define a spline with `LocalPosition`s or `GlobalPosition`s, thus giving more context to the spline. However depending on your use case, you might want to distinguish between "positions" and "differences between positions" on a type system @@ -38,40 +38,37 @@ For the most common use cases (namely `UnityEngine.Vector3`, `LocalPosition`/`Lo the `GeometricOperations` as well as some utilities are given. Thus, the classes `UnitySpline`, `LocalSpline` and `GlobalSpline` should be good starting points in most situations. -### Sampling +#### Time -A spline segment can be retrieved by indexing it at a specific `SplineSegmentIndex` like this: -```cs -Spline spline; -SplineSegment segment = spline[SplineSegmentIndex.At(3)]; -``` +Splines are also generic over their duration type. This means that you can define splines whose duration is a `float`, `double`, `UnitSystem.Duration`, `TimeSpan` or any other type that represents time. +Likewise, a spline is generic over the type of its first derivative with respect to time. For example, the derivative of a spline with handle type `GlobalPosition` and time type `Duration` would be `GlobalVelocity`. -The high-level spline API provides two ways of representing locations along a spline: -- The `SplineLocation` is the distance (in meters) to a location on the spline from the splines first interpolated handle. This distance is not the eucledian distance but the path length when traversing along the spline. +If you do not care about time, you can use so-called "uniform" splines. These splines have a duration of 1.0 (double) and their derivative is the same as their handle diff type. Each segment has the same duration. + +### Sampling + +There are three ways to represent a location on a spline: +- The aforementioned time type (e.g. `Duration`) represents the time elapsed since the spline's (temporal) beginning. Must be at least 0 and at most the spline's duration. +- The `SplineLocation` is the distance (in meters) of a location on the spline from the spline's (spatial) beginning. This distance is not the Euclidean distance to the first handle but the path length when traversing along the spline. Must be at least 0 and at most the spline's length. - The `NormalizedSplineLocation` is a number which describes a location on the spline based on spline segment - index and percentage of the next segment. For example, a value of `3.5f` indicates the point half-way between - the 4th and 5th handles. + index and the progress along that segment. For example, a value of `3.5f` indicates the point half-way along + the fourth segment. Must be at least 0 and at most the spline's segment count. -`NormalizedSplineLocations` are faster (as they are used by the underlying spline calculations), but most problems require `SplineLocations`, since they can be converted to meters. +`SplineLocations` are used a lot, especially when you want to traverse a spline at a custom speed independent of the spline's duration and the segment's individual durations. Note that they are computationally more expensive than the other two types, as they require conversion to a `NormalizedSplineLocation` to retrieve information about the spline at that location. -A spline sample can be retrieved by indexing a spline segment with a number or indexing a spline with a `SplineLocation` or `NormalizedSplineLocation`: +A spline sample can be retrieved by indexing a spline with one of the aforementioned location types. E.g.: ```cs -// Given these variables -Spline spline; -SplineSegment segment = spline[SplineSegmentIndex.At(3)]; - -// Sample at 30% between the two handles of the segment -// Must be between 0 and 1 (inclusive) -SplineSample s1 = segment.SampleAt(0.3); +// Given a non-uniform global spline +Spline spline = GlobalSpline.FromHandles(...); -// Sample at 15 meters along the spline from its beginning -SplineSample s2 = spline[SplineLocation.From(15)]; +// Sample the position at 20 seconds after the spline's beginning +GlobalPosition p = spline[20.Seconds()].Position; -// Sample within the 3rd segment at 50% between its handles -SplineSample s3 = spline[NormalizedSplineLocation.From(3.5)]; +// Sample the velocity at 50% of the 4th segment +GlobalVelocity v = spline[NormalizedSplineLocation.From(3.5)].Velocity; -// Sample within the 1st segment at 0% between its handles (e.g. at the beginning of the segment) -SplineSample s4 = spline[NormalizedSplineLocation.From(1)]; +// Sample the time at 15 meters along the spline +Duration d = spline[SplineLocation.From(15)].GlobalT; ``` ## Common Use-Cases @@ -82,23 +79,6 @@ Splines can be created in two ways: - Using the builder methods in the `GenericSpline` class or their counterparts in `UnitySpline`,`GlobalSpline` and `LocalSpline`. E.g. `UnitySpline.FromHandles(beginMargin, handles, endMargin)` - Using the extension method on `IEnumerable`, found in `GenericEnumerableToSplineExtensions` or the aforementioned specialized classes. -### General spline information: - -Here are the most commonly used operations on the fundamental spline classes: -- `.Length()` can be called on `Spline`s and `SplineSegment`s. It will return an approximation of the length of the spline's or segment's curve. (Not eucledian distance, but path length when traversing along the spline) -- `.Handles` can be called on the `Spline` to get all *interpolated handles*. The count thereof can be retrieved with `.HandleCount`. Variants that include the margin handles also exist. -- `.Position`, `.Tangent` and `.Curvature` can be called on a `SplineSample` to retrieve the respective value at that location on the spline. - -Examples: -```cs -Spline spline; - -Length length = spline.Length(); -Vector3 halfwayPosition = spline[length / 2.0f].Position; - -Vector3 tangentAtBeginning = spline[NormalizedSplineLocation.Zero].Tangent; -``` - ### Closest position on spline: To retrieve the closest point on a `Spline` (or the spline of a `WithSpline`) relative to a position in World-Space, the `.QueryClosestPositionOnSplineTo(TPos)` extension method can be used. @@ -111,16 +91,16 @@ For more options, refer to the contents of `ClosestOnSplineExtensions.cs`. ### Sampling splines: -In order to sample the positions on a spline, the `.SampleSplineEvery(...)` and `.SampleSplineBetween(...)` extension methods can be used. They can be configured to return the Positions, Tangents or Curvatures. +In order to sample many positions on a spline at once, the `.SampleSplineEvery(...)` and `.SampleSplineBetween(...)` extension methods can be used. They return a list of spline samples, from which you can retrieve data like Positions, Tangents or Curvatures. For more options, refer to `SplineSampleExtensions.cs`. ## Domain Specific Terms ### Definition of splines -In mathematics, a Catmull-Rom spline is defined by 4 handles: The 2nd and 3rd are interpolated, while the other 2 only determine the course. +In mathematics, a Catmull-Rom segment is defined by 4 handles: The 2nd and 3rd are interpolated, while the other 2 only determine the course. -The high-level spline API of this module defines a spline as the following: A Begin (margin) handle, 2 or more (normal) handles and an end (margin) handle. +The high-level spline API of this module defines a catmull-rom spline as the following: A Begin (margin) handle, 2 or more (normal) handles and an end (margin) handle. - These handles are positions - The normal handles are all interpolated in the order they appear - The margin handles are not interpolated and can be omitted in some cases (as they will be autogenerated) diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/Deleteme.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/Deleteme.cs new file mode 100644 index 00000000..5dafc25c --- /dev/null +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/Deleteme.cs @@ -0,0 +1,67 @@ +using System; +using System.Linq; +using BII.WasaBii.Core; +using BII.WasaBii.Splines; +using BII.WasaBii.Splines.CatmullRom; +using BII.WasaBii.UnitSystem; +using BII.WasaBii.Unity.Geometry; +using UnityEngine; + +namespace BII.WasaBii.Unity +{ + public class Deleteme : MonoBehaviour + { + + public Transform[] Handles; + public Transform[] Velocidies; + public float[] SectionLengths; + + public float T = 0; + public float speed = 1f; + public float durationFactor = 1f; + public bool shouldMove = false; + public Transform toMove; + + // private UnitySpline Spline => UnitySpline.FromHandlesWithVelocities( + // Handles.Select(t => t.position).Zip(Velocidies.Select(v => v.localPosition), SectionLengths.Select(f => f.Seconds()))); + + private UniformUnitySpline Spline => UniformUnitySpline.FromHandles( + Handles.Select(t => t.position), + SplineType.Chordal); + // private UnitySpline Spline => UnitySpline.FromHandles( + // Handles.Select(t => t.position).Zip(SectionLengths.Select(f => f.Seconds() * durationFactor)), + // SplineType.Chordal); + + private void OnDrawGizmos() { + var spline = Spline; + foreach (var (a, b) in spline.SampleSplinePerSegment(200).PairwiseSliding()) { + var speed = (a.Velocity.magnitude + b.Velocity.magnitude); + var t1 = (float)Math.Tanh(speed / 30); + var t2 = (float)Math.Tanh(speed / 20); + var t3 = (float)Math.Tanh(speed / 10); + Gizmos.color = Color.HSVToRGB(t1, t2, t3); + // Gizmos.color = new Color(speed/10, speed/10, speed/10); + // Gizmos.color = new Color(t, t, t); + Gizmos.DrawLine(a.Position, b.Position); + } + } + + private void Update() { + if (shouldMove) { + T += Time.deltaTime * speed; + // var sample = Spline[T.Seconds()]; + var sample = Spline[T]; + toMove.position = sample.Position; + toMove.forward = sample.Velocity; + } + } + + private void OnValidate() { + var sample = Spline[T]; + // var sample = Spline[T.Seconds()]; + toMove.position = sample.Position; + toMove.forward = sample.Velocity; + } + + } +} \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/Deleteme.cs.meta b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/Deleteme.cs.meta new file mode 100644 index 00000000..4b0630f7 --- /dev/null +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/Deleteme.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 5ebefdfef6ad45efb382f9f8ef48017a +timeCreated: 1704476345 \ No newline at end of file diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/SplineGizmo.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/SplineGizmo.cs index 3aa34d2b..8686e846 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/SplineGizmo.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Editor/SplineGizmo.cs @@ -1,4 +1,5 @@ -using System.Linq; +using System; +using System.Linq; using BII.WasaBii.Core; using BII.WasaBii.Splines; using BII.WasaBii.UnitSystem; @@ -20,7 +21,8 @@ public static class SplineGizmo { /// Draws the spline with points per segment. This means that you will see /// * spline.SegmentCount - 1 individual lines. /// - public static void DrawSegments(Spline spline, int samplesPerSegment = 10) { + public static void DrawSegments(Spline spline, int samplesPerSegment = 10) + where TTime : unmanaged, IComparable where TVel : unmanaged { foreach (var (a, b) in spline.SampleSplinePerSegment(samplesPerSegment).Select(s => s.Position).PairwiseSliding()) Gizmos.DrawLine(a, b); } @@ -29,7 +31,8 @@ public static void DrawSegments(Spline spline, int samplesPerS /// Draws the spline with points. This means that you will see /// - 1 individual lines. /// - public static void Draw(Spline spline, int samplesTotal = 10) { + public static void Draw(Spline spline, int samplesTotal = 10) + where TTime : unmanaged, IComparable where TVel : unmanaged { foreach (var (a, b) in spline.SampleSpline(samplesTotal).Select(s => s.Position).PairwiseSliding()) Gizmos.DrawLine(a, b); } @@ -38,7 +41,8 @@ public static void Draw(Spline spline, int samplesTotal = 10) /// Draws the spline such that each line will have a length of approximately . /// This means that you will see spline.Length / individual lines. /// - public static void Draw(Spline spline, Length desiredSampleLength) { + public static void Draw(Spline spline, Length desiredSampleLength) + where TTime : unmanaged, IComparable where TVel : unmanaged { foreach (var (a, b) in spline.SampleSplineEvery(desiredSampleLength).Select(s => s.Position).PairwiseSliding()) Gizmos.DrawLine(a, b); } diff --git a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Geometry/Splines/UnitySpline.cs b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Geometry/Splines/UnitySpline.cs index 22d47258..b1577134 100644 --- a/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Geometry/Splines/UnitySpline.cs +++ b/WasaBii-unity-project/Packages/WasaBii/WasaBii-Unity/Geometry/Splines/UnitySpline.cs @@ -11,93 +11,143 @@ namespace BII.WasaBii.Unity.Geometry { + [MustBeImmutable] + public interface UnityGeometricOperations : GeometricOperations + where TTime : unmanaged where TVel : unmanaged + { + Length GeometricOperations.Distance(Vector3 p0, Vector3 p1) => p0.DistanceTo(p1).Meters(); + + Vector3 GeometricOperations.Div(Vector3 diff, double d) => diff / (float) d; + Vector3 GeometricOperations.Mul(Vector3 diff, double f) => diff * (float) f; + double GeometricOperations.Dot(Vector3 a, Vector3 b) => a.Dot(b); + + Vector3 GeometricOperations.ZeroDiff => Vector3.zero; + } + [Serializable] - public sealed class UnitySpline : SpecificSplineBase { - + public sealed class UnitySpline : SpecificSplineBase { + #region Factory Methods - /// + /// [Pure] - public static UnitySpline FromHandles(IEnumerable source, SplineType? splineType = null, bool shouldLoop = false) - => new(CatmullRomSpline.FromHandlesOrThrow(source, GeometricOperations.Instance, splineType, shouldLoop)); + public static UnitySpline FromHandles(IEnumerable<(Vector3, Duration)> source, SplineType type = SplineType.Centripetal) + => new(CatmullRomSpline.FromHandlesOrThrow(source, GeometricOperations.Instance, type)); - /// + /// [Pure] public static UnitySpline FromHandles( Vector3 beginMarginHandle, - IEnumerable interpolatedHandles, + IEnumerable<(Vector3, Duration)> interpolatedHandles, Vector3 endMarginHandle, - SplineType? type = null + SplineType type = SplineType.Centripetal ) => new(CatmullRomSpline.FromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type)); - /// + /// [Pure] public static UnitySpline FromHandlesIncludingMargin( IEnumerable allHandlesIncludingMargin, - SplineType? type = null - ) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type)); + IEnumerable segmentStartTimes, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.FromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, segmentStartTimes, GeometricOperations.Instance, type)); - /// + /// [Pure] public static UnitySpline FromHandlesWithVelocities( - IEnumerable<(Vector3 position, Vector3 velocity)> handles, bool shouldLoop = false, + IEnumerable<(Vector3 position, Vector3 velocity, Duration time)> handles, bool shouldAccelerationBeContinuous = false - ) => new(BezierSpline.FromHandlesWithVelocities(handles, GeometricOperations.Instance, shouldLoop, shouldAccelerationBeContinuous)); + ) => new(BezierSpline.FromHandlesWithVelocities(handles, GeometricOperations.Instance, shouldAccelerationBeContinuous)); - /// + /// [Pure] public static UnitySpline FromHandlesWithVelocitiesAndAccelerations( - IEnumerable<(Vector3 position, Vector3 velocity, Vector3 acceleration)> handles, bool shouldLoop = false - ) => new(BezierSpline.FromHandlesWithVelocitiesAndAccelerations(handles, GeometricOperations.Instance, shouldLoop)); + IEnumerable<(Vector3 position, Vector3 velocity, Vector3 acceleration, Duration time)> handles + ) => new(BezierSpline.FromHandlesWithVelocitiesAndAccelerations(handles, GeometricOperations.Instance)); #endregion - - public UnitySpline(Spline wrapped) : base(wrapped) { } - protected override UnitySpline mkNew(Spline toWrap) => new(toWrap); - [MustBeImmutable][Serializable] - public sealed class GeometricOperations : GeometricOperations { + public UnitySpline(Spline wrapped) : base(wrapped) { } + protected override UnitySpline mkNew(Spline toWrap) => new(toWrap); + [MustBeImmutable][Serializable] + public sealed class GeometricOperations : UnityGeometricOperations + { public static readonly GeometricOperations Instance = new(); - - private GeometricOperations() { } + public Vector3 ZeroVel => Vector3.zero; + public Duration ZeroTime => Duration.Zero; + public double Div(Duration a, Duration b) => a / b; + public Vector3 Add(Vector3 a, Vector3 b) => a + b; + public Vector3 Mul(Vector3 v, Duration t) => v * (float) t.AsSeconds(); + public Vector3 Div(Vector3 d, Duration t) => d / (float) t.AsSeconds(); + 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 Vector3 Sub(Vector3 p0, Vector3 p1) => p0 - p1; + public Vector3 Lerp(Vector3 from, Vector3 to, double t) => Vector3.Lerp(from, to, (float)t); + } + + } - public Length Distance(Vector3 p0, Vector3 p1) => p0.DistanceTo(p1).Meters(); + [Serializable] + public sealed class UniformUnitySpline : SpecificSplineBase { - public Vector3 Sub(Vector3 p0, Vector3 p1) => p0 - p1; +#region Factory Methods + + /// + [Pure] + public static UniformUnitySpline FromHandles(IEnumerable source, SplineType type = SplineType.Centripetal, bool shouldLoop = false) + => new(CatmullRomSpline.UniformFromHandlesOrThrow(source, GeometricOperations.Instance, type, shouldLoop)); - public Vector3 Add(Vector3 d1, Vector3 d2) => d1 + d2; + /// + [Pure] + public static UniformUnitySpline FromHandles( + Vector3 beginMarginHandle, + IEnumerable interpolatedHandles, + Vector3 endMarginHandle, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.UniformFromHandlesOrThrow(beginMarginHandle, interpolatedHandles, endMarginHandle, GeometricOperations.Instance, type)); - public Vector3 Div(Vector3 diff, double d) => diff / (float)d; + /// + [Pure] + public static UniformUnitySpline FromHandlesIncludingMargin( + IEnumerable allHandlesIncludingMargin, + SplineType type = SplineType.Centripetal + ) => new(CatmullRomSpline.UniformFromHandlesIncludingMarginOrThrow(allHandlesIncludingMargin, GeometricOperations.Instance, type)); - public Vector3 Mul(Vector3 diff, double f) => diff * (float)f; + /// + [Pure] + public static UniformUnitySpline FromHandlesWithTangents( + IEnumerable<(Vector3 position, Vector3 tangent)> handles, bool shouldLoop = false, + bool shouldAccelerationBeContinuous = false + ) => new(BezierSpline.UniformFromHandlesWithTangents(handles, GeometricOperations.Instance, shouldLoop, shouldAccelerationBeContinuous)); - public double Dot(Vector3 a, Vector3 b) => Vector3.Dot(a, b); - - public Vector3 ZeroDiff => Vector3.zero; - + /// + [Pure] + public static UniformUnitySpline FromHandlesWithTangentsAndCurvature( + IEnumerable<(Vector3 position, Vector3 tangent, Vector3 curvature)> handles, bool shouldLoop = false + ) => new(BezierSpline.UniformFromHandlesWithTangentsAndCurvature(handles, GeometricOperations.Instance, shouldLoop)); + +#endregion + + public UniformUnitySpline(Spline wrapped) : base(wrapped) { } + protected override UniformUnitySpline mkNew(Spline toWrap) => new(toWrap); + + [MustBeImmutable][Serializable] + public sealed class GeometricOperations : UnityGeometricOperations + { + public static readonly GeometricOperations Instance = new(); + public Vector3 ZeroVel => Vector3.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 Vector3 Add(Vector3 a, Vector3 b) => a + b; + public Vector3 Mul(Vector3 a, double b) => a * (float) b; + public Vector3 Div(Vector3 a, double b) => a / (float) b; + public Vector3 Sub(Vector3 p0, Vector3 p1) => p0 - p1; public Vector3 Lerp(Vector3 from, Vector3 to, double t) => Vector3.Lerp(from, to, (float)t); } } - - public static class UnitySplineExtensions { - - /// - [Pure] public static Option<(TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult)> QueryClosestPositionOnSplinesTo( - this IEnumerable splines, - Func splineSelector, - Vector3 position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => splines.QueryClosestPositionOnSplinesTo(splineSelector, position, samples); - - /// - [Pure] public static (TWithSpline closestSpline, ClosestOnSplineQueryResult queryResult) QueryClosestPositionOnSplinesToOrThrow( - this IEnumerable splines, - Func splineSelector, - Vector3 position, - int samples = ClosestOnSplineExtensions.DefaultClosestOnSplineSamples - ) => splines.QueryClosestPositionOnSplinesToOrThrow(splineSelector, position, samples); - - } } \ No newline at end of file diff --git a/WasaBii-unity-project/WasaBii-unity-project.sln.DotSettings b/WasaBii-unity-project/WasaBii-unity-project.sln.DotSettings index 9a38735d..67b69792 100644 --- a/WasaBii-unity-project/WasaBii-unity-project.sln.DotSettings +++ b/WasaBii-unity-project/WasaBii-unity-project.sln.DotSettings @@ -94,7 +94,18 @@ Contract.Assert($EXPR$ != null, $MESSAGE$) GPS NL <Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Events"><ElementKinds><Kind Name="EVENT" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="aaBb" /></Policy></Policy> + <Policy><Descriptor Staticness="Static" AccessRightKinds="Private" Description="Static readonly fields (private)"><ElementKinds><Kind Name="READONLY_FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="aaBb"><ExtraRule Prefix="_" Suffix="" Style="aaBb" /></Policy></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Private" Description="Constant fields (private)"><ElementKinds><Kind Name="CONSTANT_FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="aaBb"><ExtraRule Prefix="_" Suffix="" Style="aaBb" /></Policy></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Type parameters"><ElementKinds><Kind Name="TYPE_PARAMETER" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="T" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="AaBb" /></Policy></Policy> + <Policy><Descriptor Staticness="Instance" AccessRightKinds="Private" Description="Instance fields (private)"><ElementKinds><Kind Name="FIELD" /><Kind Name="READONLY_FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="_" Suffix="" Style="aaBb"><ExtraRule Prefix="__" Suffix="" Style="aaBb" /></Policy></Policy> + <Policy><Descriptor Staticness="Instance" AccessRightKinds="Protected, ProtectedInternal, Internal, Public, PrivateProtected" Description="Instance fields (not private)"><ElementKinds><Kind Name="FIELD" /><Kind Name="READONLY_FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /></Policy> <Policy><Descriptor Staticness="Static, Instance" AccessRightKinds="Private, Protected, ProtectedInternal, Internal" Description="non-public methods/properties"><ElementKinds><Kind Name="METHOD" /><Kind Name="PROPERTY" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="aaBb" /></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Local variables"><ElementKinds><Kind Name="LOCAL_VARIABLE" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="aaBb" /></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Protected, ProtectedInternal, Internal, Public, PrivateProtected" Description="Constant fields (not private)"><ElementKinds><Kind Name="CONSTANT_FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /></Policy> + <Policy><Descriptor Staticness="Static" AccessRightKinds="Protected, ProtectedInternal, Internal, Public, PrivateProtected" Description="Static fields (not private)"><ElementKinds><Kind Name="FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Local functions"><ElementKinds><Kind Name="LOCAL_FUNCTION" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="aaBb" /></Policy></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Methods"><ElementKinds><Kind Name="METHOD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="aaBb" /></Policy></Policy> <Policy><Descriptor Staticness="Static, Instance" AccessRightKinds="Private, Protected, ProtectedInternal, Internal" Description="non-public readonly fields"><ElementKinds><Kind Name="READONLY_FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="aaBb" /></Policy> <Policy Inspect="True" Prefix="" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="aaBb" /></Policy> <Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /> @@ -109,13 +120,17 @@ Contract.Assert($EXPR$ != null, $MESSAGE$) <Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /> <Policy Inspect="True" Prefix="T" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="AaBb" /></Policy> <Policy><Descriptor Staticness="Static, Instance" AccessRightKinds="Private, Protected, ProtectedInternal, Internal" Description="non-public mutable fields"><ElementKinds><Kind Name="FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="_" Suffix="" Style="aaBb" /></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Interfaces"><ElementKinds><Kind Name="INTERFACE" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /></Policy> + <Policy><Descriptor Staticness="Any" AccessRightKinds="Any" Description="Properties"><ElementKinds><Kind Name="PROPERTY" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb"><ExtraRule Prefix="" Suffix="" Style="aaBb" /></Policy></Policy> <Policy><Descriptor Staticness="Static, Instance" AccessRightKinds="Public" Description="public methods/properties"><ElementKinds><Kind Name="METHOD" /><Kind Name="PROPERTY" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="" Suffix="" Style="AaBb" /></Policy> + <Policy><Descriptor Staticness="Static" AccessRightKinds="Private" Description="Static fields (private)"><ElementKinds><Kind Name="FIELD" /></ElementKinds></Descriptor><Policy Inspect="True" Prefix="_" Suffix="" Style="aaBb"><ExtraRule Prefix="" Suffix="" Style="aaBb" /></Policy></Policy> Disable Disable True True True True + True True True True