diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs new file mode 100644 index 0000000..aa31a0b --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs @@ -0,0 +1,532 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms; + +/// 按方向段局部弧长构造 C2 连续的分段五次 Hermite 原始几何候选。 +internal sealed class PiecewiseQuinticSmoother : IPathSmoother +{ + private const int SamplesPerInterval = 8; + + /// + public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic; + + /// + public SmoothingCandidate Smooth( + SmoothingAlgorithmInput input, + double effectiveStrength, + CancellationToken cancellationToken) + { + if (input == null || input.OriginalPath == null || input.Options == null || + !NumericGuard.IsPositiveFinite(effectiveStrength) || + !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) || + input.MinimumClearanceReserveMeters < 0d || + !NumericGuard.IsPositiveFinite(input.Options.QuinticKnotSpacingMeters) || + !NumericGuard.IsPositiveFinite(input.Options.QuinticMinimumKnotSpacingMeters) || + input.Options.QuinticKnotSpacingMeters < input.Options.QuinticMinimumKnotSpacingMeters) + { + return SmoothingCandidate.Failed("五次 Hermite 输入、强度、净空预留或结点间距无效。"); + } + + var candidateSegments = new List(input.OriginalPath.Segments.Count); + for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++) + { + cancellationToken.ThrowIfCancellationRequested(); + PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex]; + if (!TrySmoothSegment( + sourceSegment, + effectiveStrength, + input.MinimumClearanceReserveMeters, + input.Options.QuinticKnotSpacingMeters, + input.Options.QuinticMinimumKnotSpacingMeters, + cancellationToken, + out IReadOnlyList points, + out string reason, + out SmoothingCandidateStatus status)) + { + return status == SmoothingCandidateStatus.RetryableInfeasible + ? SmoothingCandidate.RetryableInfeasible(reason) + : SmoothingCandidate.Failed(reason); + } + + candidateSegments.Add(new PreparedDirectionSegment( + sourceSegment.SegmentIndex, + sourceSegment.Direction, + points, + sourceSegment.StartsAtGearSwitch, + sourceSegment.EndsAtGearSwitch)); + } + + return SmoothingCandidate.Success(candidateSegments); + } + + private static bool TrySmoothSegment( + PreparedDirectionSegment sourceSegment, + double effectiveStrength, + double reserveMeters, + double knotSpacingMeters, + double minimumKnotSpacingMeters, + CancellationToken cancellationToken, + out IReadOnlyList result, + out string reason, + out SmoothingCandidateStatus status) + { + result = null; + reason = string.Empty; + status = SmoothingCandidateStatus.Failed; + if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count < 2) + { + reason = "五次 Hermite 方向段至少需要两个锚点。"; + return false; + } + + IReadOnlyList anchors = sourceSegment.Points; + if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false; + + if (!TryCreateKnots( + anchors, + sourceSegment.Direction, + effectiveStrength, + knotSpacingMeters, + minimumKnotSpacingMeters, + cancellationToken, + out List knots, + out reason)) + { + return false; + } + + // Each physical acceleration is blended once into its shared knot and then scaled by + // the left/right local interval independently. Reusing this value is what makes the + // curve C2 with respect to local arc length, even for nonuniform final intervals. + if (!TryAssignSharedAccelerations(knots, out reason)) return false; + if (!TryCreateIntervals(knots, out List intervals, out reason)) return false; + + var sampled = new List(1 + intervals.Count * SamplesPerInterval); + for (int intervalIndex = 0; intervalIndex < intervals.Count; intervalIndex++) + { + cancellationToken.ThrowIfCancellationRequested(); + QuinticInterval interval = intervals[intervalIndex]; + int firstSample = intervalIndex == 0 ? 0 : 1; + for (int sampleIndex = firstSample; sampleIndex <= SamplesPerInterval; sampleIndex++) + { + cancellationToken.ThrowIfCancellationRequested(); + double parameter = (double)sampleIndex / SamplesPerInterval; + double referenceArcLength = interval.Start.ArcLength + parameter * interval.Length; + if (!NumericGuard.IsFinite(referenceArcLength)) + { + reason = "五次 Hermite 采样参考弧长无效。"; + return false; + } + + if (!PathReferenceInterpolator.TryInterpolateByArcLength( + anchors, + referenceArcLength, + out SmoothingPoint2D reference, + out reason)) + { + return false; + } + + Point2D evaluated; + if (sampleIndex == 0) + evaluated = interval.Start.Position; + else if (sampleIndex == SamplesPerInterval) + evaluated = interval.End.Position; + else if (!interval.TryEvaluate(parameter, out evaluated, out Point2D derivative, out Point2D secondDerivative)) + { + reason = "五次 Hermite 采样产生非有限位置或导数。"; + return false; + } + + double displacement = Distance(evaluated, reference); + if (!NumericGuard.IsFinite(displacement)) + { + reason = "五次 Hermite 采样位移无效。"; + return false; + } + double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters); + if (displacement > allowedDisplacement) + { + reason = "五次 Hermite 采样点超过对应局部弧长参考点的允许移动范围。"; + status = SmoothingCandidateStatus.RetryableInfeasible; + return false; + } + + bool firstEndpoint = intervalIndex == 0 && sampleIndex == 0; + bool lastEndpoint = intervalIndex == intervals.Count - 1 && sampleIndex == SamplesPerInterval; + if (firstEndpoint) + { + sampled.Add(anchors[0]); + } + else if (lastEndpoint) + { + sampled.Add(anchors[anchors.Count - 1]); + } + else + { + sampled.Add(new SmoothingPoint2D( + evaluated.X, + evaluated.Y, + reference.ArcLength, + reference.Heading, + reference.UnwrappedHeading, + reference.BodyClearance, + false, + SmoothedPathPointSource.Interpolated)); + } + } + } + + result = sampled; + return true; + } + + private static bool ValidateAnchors( + IReadOnlyList anchors, + CancellationToken cancellationToken, + out string reason) + { + reason = string.Empty; + for (int index = 0; index < anchors.Count; index++) + { + cancellationToken.ThrowIfCancellationRequested(); + SmoothingPoint2D point = anchors[index]; + if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) || + !NumericGuard.IsFinite(point.ArcLength) || !NumericGuard.IsFinite(point.Heading) || + !NumericGuard.IsFinite(point.UnwrappedHeading) || !NumericGuard.IsFinite(point.BodyClearance) || + point.ArcLength < 0d || point.BodyClearance < 0d || + (index > 0 && point.ArcLength <= anchors[index - 1].ArcLength)) + { + reason = "五次 Hermite 方向段包含非有限、非递增弧长或无效净空的锚点。"; + return false; + } + } + return true; + } + + private static bool TryCreateKnots( + IReadOnlyList anchors, + TravelDirection direction, + double effectiveStrength, + double knotSpacingMeters, + double minimumKnotSpacingMeters, + CancellationToken cancellationToken, + out List knots, + out string reason) + { + knots = new List(); + reason = string.Empty; + double startArcLength = anchors[0].ArcLength; + double endArcLength = anchors[anchors.Count - 1].ArcLength; + double totalLength = endArcLength - startArcLength; + if (!NumericGuard.IsPositiveFinite(totalLength) || totalLength < minimumKnotSpacingMeters) + { + reason = "五次 Hermite 方向段短于配置的最小结点间距。"; + return false; + } + + if (!TryCreateKnot(anchors[0], direction, effectiveStrength, out Knot first)) + { + reason = "五次 Hermite 起点结点或行进切向无效。"; + return false; + } + knots.Add(first); + + double targetArcLength = startArcLength + knotSpacingMeters; + while (targetArcLength < endArcLength) + { + cancellationToken.ThrowIfCancellationRequested(); + if (!NumericGuard.IsFinite(targetArcLength)) + { + reason = "五次 Hermite 内部结点弧长无效。"; + return false; + } + if (!PathReferenceInterpolator.TryInterpolateByArcLength( + anchors, + targetArcLength, + out SmoothingPoint2D reference, + out reason) || + !TryCreateKnot(reference, direction, effectiveStrength, out Knot knot)) + { + if (string.IsNullOrEmpty(reason)) reason = "五次 Hermite 内部结点或行进切向无效。"; + return false; + } + knots.Add(knot); + targetArcLength += knotSpacingMeters; + } + + if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last)) + { + reason = "五次 Hermite 终点结点或行进切向无效。"; + return false; + } + knots.Add(last); + + for (int index = 1; index < knots.Count; index++) + { + double intervalLength = knots[index].ArcLength - knots[index - 1].ArcLength; + if (!NumericGuard.IsPositiveFinite(intervalLength) || intervalLength < minimumKnotSpacingMeters) + { + reason = "五次 Hermite 结点间隔无效或短于配置的最小间距。"; + return false; + } + } + return true; + } + + private static bool TryCreateKnot( + SmoothingPoint2D reference, + TravelDirection direction, + double effectiveStrength, + out Knot knot) + { + knot = default; + if (reference == null || !NumericGuard.IsFinite(reference.X) || !NumericGuard.IsFinite(reference.Y) || + !NumericGuard.IsFinite(reference.ArcLength) || !NumericGuard.IsFinite(reference.Heading)) + { + return false; + } + + double travelHeading = direction == TravelDirection.Forward + ? reference.Heading + : reference.Heading - Math.PI; + double tangentX = Math.Cos(travelHeading); + double tangentY = Math.Sin(travelHeading); + if (!NumericGuard.IsFinite(tangentX) || !NumericGuard.IsFinite(tangentY)) return false; + + var velocity = new Point2D(tangentX * effectiveStrength, tangentY * effectiveStrength); + if (!velocity.IsFinite) return false; + knot = new Knot(reference.ArcLength, new Point2D(reference.X, reference.Y), velocity); + return true; + } + + private static bool TryAssignSharedAccelerations(List knots, out string reason) + { + reason = string.Empty; + for (int index = 0; index < knots.Count; index++) + { + Point2D acceleration; + if (index == 0) + { + if (!TryAcceleration(knots[0], knots[1], out acceleration)) + { + reason = "五次 Hermite 起点加速度无效。"; + return false; + } + } + else if (index == knots.Count - 1) + { + if (!TryAcceleration(knots[index - 1], knots[index], out acceleration)) + { + reason = "五次 Hermite 终点加速度无效。"; + return false; + } + } + else + { + if (!TryAcceleration(knots[index - 1], knots[index], out Point2D left) || + !TryAcceleration(knots[index], knots[index + 1], out Point2D right)) + { + reason = "五次 Hermite 共享结点加速度无效。"; + return false; + } + acceleration = new Point2D((left.X + right.X) / 2d, (left.Y + right.Y) / 2d); + if (!acceleration.IsFinite) + { + reason = "五次 Hermite 共享结点加速度混合产生非有限数值。"; + return false; + } + } + + knots[index] = knots[index].WithAcceleration(acceleration); + } + return true; + } + + private static bool TryAcceleration(Knot start, Knot end, out Point2D acceleration) + { + acceleration = default; + double intervalLength = end.ArcLength - start.ArcLength; + if (!NumericGuard.IsPositiveFinite(intervalLength)) return false; + acceleration = new Point2D( + (end.Velocity.X - start.Velocity.X) / intervalLength, + (end.Velocity.Y - start.Velocity.Y) / intervalLength); + return acceleration.IsFinite; + } + + private static bool TryCreateIntervals( + IReadOnlyList knots, + out List intervals, + out string reason) + { + intervals = new List(knots.Count - 1); + reason = string.Empty; + for (int index = 1; index < knots.Count; index++) + { + if (!QuinticInterval.TryCreate(knots[index - 1], knots[index], out QuinticInterval interval)) + { + reason = "五次 Hermite 系数、端点导数或结点区间无效。"; + return false; + } + intervals.Add(interval); + } + return true; + } + + private static double Distance(Point2D point, SmoothingPoint2D reference) + { + double deltaX = point.X - reference.X; + double deltaY = point.Y - reference.Y; + return Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + } + + private readonly struct Knot + { + internal Knot(double arcLength, Point2D position, Point2D velocity) + { + ArcLength = arcLength; + Position = position; + Velocity = velocity; + Acceleration = default; + } + + internal double ArcLength { get; } + + internal Point2D Position { get; } + + internal Point2D Velocity { get; } + + internal Point2D Acceleration { get; } + + internal Knot WithAcceleration(Point2D acceleration) + { + return new Knot(ArcLength, Position, Velocity, acceleration); + } + + private Knot(double arcLength, Point2D position, Point2D velocity, Point2D acceleration) + { + ArcLength = arcLength; + Position = position; + Velocity = velocity; + Acceleration = acceleration; + } + } + + private readonly struct QuinticInterval + { + private QuinticInterval(Knot start, Knot end, Point2D c0, Point2D c1, Point2D c2, Point2D c3, Point2D c4, Point2D c5) + { + Start = start; + End = end; + Length = end.ArcLength - start.ArcLength; + _c0 = c0; + _c1 = c1; + _c2 = c2; + _c3 = c3; + _c4 = c4; + _c5 = c5; + } + + private readonly Point2D _c0; + private readonly Point2D _c1; + private readonly Point2D _c2; + private readonly Point2D _c3; + private readonly Point2D _c4; + private readonly Point2D _c5; + + internal Knot Start { get; } + + internal Knot End { get; } + + internal double Length { get; } + + internal static bool TryCreate(Knot start, Knot end, out QuinticInterval interval) + { + interval = default; + double length = end.ArcLength - start.ArcLength; + if (!NumericGuard.IsPositiveFinite(length) || !start.Position.IsFinite || !end.Position.IsFinite || + !start.Velocity.IsFinite || !end.Velocity.IsFinite || + !start.Acceleration.IsFinite || !end.Acceleration.IsFinite) + { + return false; + } + + Point2D c0 = start.Position; + Point2D c1 = Scale(start.Velocity, length); + Point2D c2 = Scale(start.Acceleration, length * length / 2d); + Point2D difference = Subtract(end.Position, start.Position); + Point2D endVelocity = Scale(end.Velocity, length); + Point2D startAcceleration = Scale(start.Acceleration, length * length); + Point2D endAcceleration = Scale(end.Acceleration, length * length); + Point2D c3 = Add( + Add(Scale(difference, 10d), Scale(c1, -6d)), + Add(Scale(endVelocity, -4d), Add(Scale(startAcceleration, -1.5d), Scale(endAcceleration, 0.5d)))); + Point2D c4 = Add( + Add(Scale(difference, -15d), Scale(c1, 8d)), + Add(Scale(endVelocity, 7d), Add(Scale(startAcceleration, 1.5d), Scale(endAcceleration, -1d)))); + Point2D c5 = Add( + Add(Scale(difference, 6d), Add(Scale(c1, -3d), Scale(endVelocity, -3d))), + Add(Scale(startAcceleration, -0.5d), Scale(endAcceleration, 0.5d))); + if (!c0.IsFinite || !c1.IsFinite || !c2.IsFinite || !c3.IsFinite || !c4.IsFinite || !c5.IsFinite) + return false; + + interval = new QuinticInterval(start, end, c0, c1, c2, c3, c4, c5); + return interval.TryEvaluate(0d, out _, out _, out _) && interval.TryEvaluate(1d, out _, out _, out _); + } + + internal bool TryEvaluate(double parameter, out Point2D position, out Point2D derivative, out Point2D secondDerivative) + { + position = default; + derivative = default; + secondDerivative = default; + if (!NumericGuard.IsFinite(parameter) || parameter < 0d || parameter > 1d) return false; + + double t2 = parameter * parameter; + double t3 = t2 * parameter; + double t4 = t3 * parameter; + double t5 = t4 * parameter; + position = Add(Add(Add(_c0, Scale(_c1, parameter)), Add(Scale(_c2, t2), Scale(_c3, t3))), + Add(Scale(_c4, t4), Scale(_c5, t5))); + derivative = Add(Add(_c1, Scale(_c2, 2d * parameter)), + Add(Scale(_c3, 3d * t2), Add(Scale(_c4, 4d * t3), Scale(_c5, 5d * t4)))); + secondDerivative = Add(Scale(_c2, 2d), + Add(Scale(_c3, 6d * parameter), Add(Scale(_c4, 12d * t2), Scale(_c5, 20d * t3)))); + return position.IsFinite && derivative.IsFinite && secondDerivative.IsFinite; + } + } + + private readonly struct Point2D + { + internal Point2D(double x, double y) + { + X = x; + Y = y; + } + + internal double X { get; } + + internal double Y { get; } + + internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y); + } + + private static Point2D Add(Point2D left, Point2D right) + { + return new Point2D(left.X + right.X, left.Y + right.Y); + } + + private static Point2D Subtract(Point2D left, Point2D right) + { + return new Point2D(left.X - right.X, left.Y - right.Y); + } + + private static Point2D Scale(Point2D point, double scale) + { + return new Point2D(point.X * scale, point.Y * scale); + } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 b/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 new file mode 100644 index 0000000..59f027d --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 @@ -0,0 +1,313 @@ +param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll')) + +$ErrorActionPreference = 'Stop' +$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath)) + +function Assert-True($Actual, [string]$Message) { + if (-not $Actual) { throw $Message } +} + +function Assert-Equal($Expected, $Actual, [string]$Message) { + if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } +} + +function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) { + if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) { + throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance" + } +} + +function Get-RequiredType([string]$Name) { + return $assembly.GetType($Name, $true) +} + +function Get-PropertyValue($Instance, [string]$Name) { + $property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic') + Assert-True ($null -ne $property) ("Missing property: " + $Name) + return $property.GetValue($Instance) +} + +function New-Point( + [double]$X, + [double]$Y, + [double]$ArcLength, + [double]$Heading, + [double]$BodyClearance = 1.0, + [bool]$IsGearSwitch = $false) { + return [Activator]::CreateInstance($pointType, @( + $X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor)) +} + +function New-DirectionSegment( + [int]$Index, + $Direction, + [object[]]$Points, + [bool]$StartsAtGearSwitch = $false, + [bool]$EndsAtGearSwitch = $false) { + $typedPoints = [Array]::CreateInstance($pointType, $Points.Count) + for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) { + $typedPoints.SetValue($Points[$pointIndex], $pointIndex) + } + return [Activator]::CreateInstance($segmentType, @( + $Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch)) +} + +function New-EmptyMap { + $request = [Activator]::CreateInstance($mapRequestType) + $request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000)) + $request.ResolutionMm = [single]50 + $request.AllowExplicitEmptyMap = $true + $map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map + Assert-True ($null -ne $map) 'Quintic test must create an explicit empty planning map.' + return $map +} + +function New-AlgorithmInput( + [object[]]$Segments, + [double]$ReserveMeters, + [double]$KnotSpacingMeters = 1.0, + [double]$MinimumKnotSpacingMeters = 0.10) { + $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count) + for ($index = 0; $index -lt $Segments.Count; $index++) { + $typedSegments.SetValue($Segments[$index], $index) + } + $preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments)) + $vehicle = [Activator]::CreateInstance($vehicleType) + $vehicle.LengthMeters = [double]0.20 + $vehicle.WidthMeters = [double]0.20 + $vehicle.SafetyMarginMeters = [double]0.0 + $vehicle.MaximumCurvaturePerMeter = [double]100.0 + $vehicle.MinimumTurningRadiusMeters = [double]0.01 + $configuration = [Activator]::CreateInstance($configurationType) + $configuration.PiecewiseQuintic.KnotSpacingMeters = $KnotSpacingMeters + $configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = $MinimumKnotSpacingMeters + $options = $optionsConstructor.Invoke(@($configuration)) + return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options)) +} + +function Invoke-Candidate( + [object[]]$Segments, + [double]$ReserveMeters = 0.0, + [double]$KnotSpacingMeters = 1.0, + [double]$MinimumKnotSpacingMeters = 0.10) { + return $smoothMethod.Invoke($smoother, @( + (New-AlgorithmInput $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters), + [double]1.0, [Threading.CancellationToken]::None)) +} + +function Invoke-Smoothing( + [object[]]$Segments, + [double]$ReserveMeters = 0.0, + [double]$KnotSpacingMeters = 1.0, + [double]$MinimumKnotSpacingMeters = 0.10) { + $candidate = Invoke-Candidate $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters + Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Quintic smoothing must produce a candidate for the deterministic fixture.' + return @(Get-PropertyValue $candidate 'Segments') +} + +function Get-PointDistance($Left, $Right) { + $deltaX = $Left.X - $Right.X + $deltaY = $Left.Y - $Right.Y + return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY) +} + +function Get-Reference([object[]]$Source, [double]$ArcLength) { + $typedPoints = [Array]::CreateInstance($pointType, $Source.Count) + for ($index = 0; $index -lt $Source.Count; $index++) { $typedPoints.SetValue($Source[$index], $index) } + $arguments = [object[]]@($typedPoints, $ArcLength, $null, $null) + Assert-True $interpolateMethod.Invoke($null, $arguments) 'Quintic test must resolve every sampled local-arc reference.' + return $arguments[2] +} + +function Get-PointAtArcLength([object[]]$Points, [double]$ArcLength) { + foreach ($point in $Points) { + if ([Math]::Abs($point.ArcLength - $ArcLength) -lt 0.000000000001) { return $point } + } + throw "Missing quintic sample at arc length $ArcLength" +} + +function Get-EndpointDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) { + $firstCoefficients = @((-137.0 / 60.0), 5.0, -5.0, (10.0 / 3.0), (-5.0 / 4.0), (1.0 / 5.0)) + $x = 0.0 + $y = 0.0 + for ($index = 0; $index -lt 6; $index++) { + $sampleIndex = if ($AtStart) { $index } else { 5 - $index } + $sign = if ($AtStart) { 1.0 } else { -1.0 } + $x += $firstCoefficients[$index] * $Samples[$sampleIndex].X + $y += $firstCoefficients[$index] * $Samples[$sampleIndex].Y + } + return [PSCustomObject]@{ X = $sign * $x / $StepMeters; Y = $sign * $y / $StepMeters } +} + +function Get-EndpointSecondDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) { + $coefficients = @((15.0 / 4.0), (-77.0 / 6.0), (107.0 / 6.0), -13.0, (61.0 / 12.0), (-5.0 / 6.0)) + $x = 0.0 + $y = 0.0 + for ($index = 0; $index -lt 6; $index++) { + $sampleIndex = if ($AtStart) { $index } else { 5 - $index } + $x += $coefficients[$index] * $Samples[$sampleIndex].X + $y += $coefficients[$index] * $Samples[$sampleIndex].Y + } + return [PSCustomObject]@{ X = $x / ($StepMeters * $StepMeters); Y = $y / ($StepMeters * $StepMeters) } +} + +function Get-IntervalSamples([object[]]$Points, [double]$StartArcLength, [double]$EndArcLength, [bool]$FromStart) { + $intervalLength = $EndArcLength - $StartArcLength + $result = @() + for ($index = 0; $index -lt 6; $index++) { + $parameter = if ($FromStart) { $index / 8.0 } else { (3.0 + $index) / 8.0 } + $result += Get-PointAtArcLength $Points ($StartArcLength + $parameter * $intervalLength) + } + return $result +} + +function Get-QuinticPositionFromInteriorSamples( + [object[]]$Points, + [double]$StartArcLength, + [double]$EndArcLength, + [double[]]$Parameters, + [double]$TargetParameter) { + $intervalLength = $EndArcLength - $StartArcLength + $x = 0.0 + $y = 0.0 + for ($index = 0; $index -lt $Parameters.Count; $index++) { + $weight = 1.0 + for ($otherIndex = 0; $otherIndex -lt $Parameters.Count; $otherIndex++) { + if ($index -ne $otherIndex) { + $weight *= ($TargetParameter - $Parameters[$otherIndex]) / ($Parameters[$index] - $Parameters[$otherIndex]) + } + } + $sample = Get-PointAtArcLength $Points ($StartArcLength + $Parameters[$index] * $intervalLength) + $x += $weight * $sample.X + $y += $weight * $sample.Y + } + return [PSCustomObject]@{ X = $x; Y = $y } +} + +$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.' +$processing = $root + 'Processing.' +$algorithms = $root + 'Algorithms.' +$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.' + +$smootherType = Get-RequiredType ($algorithms + 'PiecewiseQuinticSmoother') +$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D') +$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment') +$preparedPathType = Get-RequiredType ($processing + 'PreparedPath') +$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput') +$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot') +$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration') +$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator') +$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters') +$directionType = Get-RequiredType ($coarsePath + 'TravelDirection') +$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource') +$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm' +$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap' +$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest' +$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory' + +$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, + @($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null) +Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable quintic options and clearance reserve.' +$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null) +Assert-True ($null -ne $optionsConstructor) 'Quintic tests must create immutable options snapshots.' +$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic') +Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose local-arc interpolation.' +$smoother = [Activator]::CreateInstance($smootherType, $true) +$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public') +Assert-True ($null -ne $smoothMethod) 'PiecewiseQuinticSmoother must implement the internal smoother contract.' +Assert-Equal 'PiecewiseQuintic' $smoother.Method.ToString() 'Quintic smoother must identify its public smoothing method.' + +$forward = [Enum]::Parse($directionType, 'Forward') +$reverse = [Enum]::Parse($directionType, 'Reverse') +$anchor = [Enum]::Parse($sourceType, 'Anchor') + +# The 1.0 m local-arc knot spacing creates shared knots at s=1 and s=2. +# The generated 1/8 samples allow exact one-sided quintic derivative reconstruction. +$continuitySource = @( + (New-Point 0.00 0.00 0.00 0.00), + (New-Point 0.50 0.00 0.50 0.00), + (New-Point 1.00 0.00 1.00 0.00), + (New-Point 1.00 0.50 1.50 ([Math]::PI / 2.0)), + (New-Point 1.00 1.00 2.00 ([Math]::PI / 2.0)), + (New-Point 1.30 1.30 2.40 ([Math]::PI / 4.0))) +$continuityOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)))[0].Points +foreach ($sharedArcLength in @(1.0, 2.0)) { + $leftSamples = Get-IntervalSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength $false + $rightEnd = if ($sharedArcLength -eq 2.0) { 2.4 } else { $sharedArcLength + 1.0 } + $rightSamples = Get-IntervalSamples $continuityOutput $sharedArcLength $rightEnd $true + $leftPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength ` + @((2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0), (7.0 / 8.0)) 1.0 + $rightPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput $sharedArcLength $rightEnd ` + @((1.0 / 8.0), (2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0)) 0.0 + $leftFirst = Get-EndpointDerivative $leftSamples (1.0 / 8.0) $false + $rightFirst = Get-EndpointDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true + $leftSecond = Get-EndpointSecondDerivative $leftSamples (1.0 / 8.0) $false + $rightSecond = Get-EndpointSecondDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true + Assert-Near $leftPosition.X $rightPosition.X 0.000001 'Shared knot X position must match from both quintic intervals.' + Assert-Near $leftPosition.Y $rightPosition.Y 0.000001 'Shared knot Y position must match from both quintic intervals.' + Assert-Near $leftFirst.X $rightFirst.X 0.000001 'Shared knot X first derivative must be C1.' + Assert-Near $leftFirst.Y $rightFirst.Y 0.000001 'Shared knot Y first derivative must be C1.' + Assert-Near $leftSecond.X $rightSecond.X 0.000001 'Shared knot X second derivative must be C2.' + Assert-Near $leftSecond.Y $rightSecond.Y 0.000001 'Shared knot Y second derivative must be C2.' +} + +$firstOutput = $continuityOutput[0] +$lastOutput = $continuityOutput[$continuityOutput.Count - 1] +Assert-Near $continuitySource[0].X $firstOutput.X 0.0 'Quintic start X must remain exact.' +Assert-Near $continuitySource[0].Y $firstOutput.Y 0.0 'Quintic start Y must remain exact.' +Assert-Near $continuitySource[$continuitySource.Count - 1].X $lastOutput.X 0.0 'Quintic end X must remain exact.' +Assert-Near $continuitySource[$continuitySource.Count - 1].Y $lastOutput.Y 0.0 'Quintic end Y must remain exact.' +foreach ($point in $continuityOutput) { + $reference = Get-Reference $continuitySource $point.ArcLength + Assert-True ((Get-PointDistance $point $reference) -le ($reference.BodyClearance + 0.000000000001)) 'Every quintic sample must remain inside its local reference movement bound.' +} + +# Separate prepared direction segments must retain their exact duplicated switch pose and topology. +$reverseSource = @( + (New-Point 1.30 1.30 0.00 ([Math]::PI / 4.0) 1.0 $true), + (New-Point 1.30 0.80 0.50 ([Math]::PI / 2.0)), + (New-Point 1.30 0.30 1.00 ([Math]::PI / 2.0))) +$switchOutput = @(Invoke-Smoothing @( + (New-DirectionSegment 0 $forward $continuitySource $false $true), + (New-DirectionSegment 1 $reverse $reverseSource $true $false))) +Assert-Equal 2 $switchOutput.Count 'Quintic smoothing must retain separate direction segments.' +Assert-True $switchOutput[0].EndsAtGearSwitch 'Forward quintic segment must retain its gear-switch boundary flag.' +Assert-True $switchOutput[1].StartsAtGearSwitch 'Reverse quintic segment must retain its gear-switch boundary flag.' +$leftSwitch = $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] +$rightSwitch = $switchOutput[1].Points[0] +Assert-Near $leftSwitch.X $rightSwitch.X 0.0 'Quintic smoothing must preserve switch X exactly.' +Assert-Near $leftSwitch.Y $rightSwitch.Y 0.0 'Quintic smoothing must preserve switch Y exactly.' + +# Spacing selects local-arc knots, while a valid minimum spacing does not change that selection. +$oneMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.10)[0].Points +$halfMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 0.50 0.10)[0].Points +$largeMinimumOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.30)[0].Points +Assert-True ($halfMeterOutput.Count -gt $oneMeterOutput.Count) 'Custom knot spacing must create additional local-arc knot intervals.' +Assert-Equal $oneMeterOutput.Count $largeMinimumOutput.Count 'Valid minimum knot spacing must not change knot selection.' +for ($index = 0; $index -lt $oneMeterOutput.Count; $index++) { + Assert-Near $oneMeterOutput[$index].X $largeMinimumOutput[$index].X 0.000000000001 'Minimum knot spacing must not change valid quintic X geometry.' + Assert-Near $oneMeterOutput[$index].Y $largeMinimumOutput[$index].Y 0.000000000001 'Minimum knot spacing must not change valid quintic Y geometry.' +} + +$shortSource = @( + (New-Point 0.00 0.00 0.00 0.00), + (New-Point 0.05 0.00 0.05 0.00)) +$shortCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $shortSource)) 0.0 1.0 0.10 +Assert-Equal 'Failed' (Get-PropertyValue $shortCandidate 'Status').ToString() 'A segment shorter than the configured minimum knot spacing must fail terminally.' +Assert-True (-not (Get-PropertyValue $shortCandidate 'Succeeded')) 'A degenerate short quintic segment must not be executable.' +Assert-Equal 0 (Get-PropertyValue $shortCandidate 'Segments').Count 'A terminal quintic degeneracy must publish no geometry.' + +# Local-arc reference mapping, rather than sample index/global distance, must reject this unsafe nonuniform path. +$nonuniformUnsafeSource = @( + (New-Point 0.0 0.0 0.0 0.0 0.50), + (New-Point 1.0 0.0 4.0 0.0 0.50), + (New-Point 2.0 0.0 5.0 0.0 0.50), + (New-Point 3.0 0.0 6.0 0.0 0.50), + (New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50), + (New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50)) +$nonuniformUnsafeCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformUnsafeSource)) 0.0 5.0 0.10 +Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformUnsafeCandidate 'Status').ToString() 'Unsafe nonuniform local-arc quintic movement must be retryable.' +Assert-True (-not (Get-PropertyValue $nonuniformUnsafeCandidate 'Succeeded')) 'Unsafe nonuniform quintic geometry must not be executable.' +Assert-Equal 0 (Get-PropertyValue $nonuniformUnsafeCandidate 'Segments').Count 'Retryable quintic infeasibility must publish no geometry.' + +Write-Output 'Path smoothing piecewise quintic checks passed.'