From c893abe7d2513a576c102fe5504af8bd670f23c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Wed, 29 Jul 2026 13:21:13 +0800 Subject: [PATCH] feat: add local cubic bezier smoother --- .../Algorithms/LocalCubicBezierSmoother.cs | 395 ++++++++++++++++++ .../tests/verify_path_smoothing_bezier.ps1 | 278 ++++++++++++ 2 files changed, 673 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs create mode 100644 ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs new file mode 100644 index 0000000..6593359 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs @@ -0,0 +1,395 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms; + +/// 在方向段内以局部三次 Bézier 连接替换明显转角。 +internal sealed class LocalCubicBezierSmoother : IPathSmoother +{ + private const double WindowToleranceMeters = 1e-9d; + + /// + public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier; + + /// + 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) + { + return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。"); + } + + 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, + input.Options.BezierCornerHeadingThresholdRadians, + input.Options.BezierMaximumWindowLengthMeters, + input.Options.BezierHandleLengthRatio, + effectiveStrength, + input.MinimumClearanceReserveMeters, + 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 cornerThresholdRadians, + double maximumWindowLengthMeters, + double handleLengthRatio, + double strength, + double reserveMeters, + 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 == 0) + { + reason = "Bézier 方向段为空。"; + return false; + } + + IReadOnlyList anchors = sourceSegment.Points; + if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false; + if (anchors.Count < 3) + { + result = anchors; + return true; + } + + if (!TryCreateMergedWindows( + anchors, + cornerThresholdRadians, + maximumWindowLengthMeters, + cancellationToken, + out List windows, + out reason)) + { + return false; + } + if (windows.Count == 0) + { + result = anchors; + return true; + } + + var output = new List(anchors.Count); + int anchorIndex = 0; + for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++) + { + cancellationToken.ThrowIfCancellationRequested(); + Window window = windows[windowIndex]; + while (anchorIndex <= window.StartIndex) + { + output.Add(anchors[anchorIndex]); + anchorIndex++; + } + + if (!TryAppendWindowInterior( + anchors, + window, + handleLengthRatio, + strength, + reserveMeters, + output, + cancellationToken, + out reason, + out status)) + { + return false; + } + + output.Add(anchors[window.EndIndex]); + anchorIndex = window.EndIndex + 1; + } + + while (anchorIndex < anchors.Count) + { + output.Add(anchors[anchorIndex]); + anchorIndex++; + } + + result = output; + 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) || point.ArcLength < 0d || + !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) || + !NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d || + (index > 0 && point.ArcLength <= anchors[index - 1].ArcLength)) + { + reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。"; + return false; + } + } + return true; + } + + private static bool TryCreateMergedWindows( + IReadOnlyList anchors, + double cornerThresholdRadians, + double maximumWindowLengthMeters, + CancellationToken cancellationToken, + out List windows, + out string reason) + { + windows = new List(); + reason = string.Empty; + for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++) + { + cancellationToken.ThrowIfCancellationRequested(); + if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) || + !TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent)) + { + reason = "Bézier 转角包含零长度或非有限行进切向。"; + return false; + } + + double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X; + double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y; + double turnRadians = Math.Atan2(Math.Abs(cross), dot); + if (!NumericGuard.IsFinite(turnRadians)) + { + reason = "Bézier 转角计算产生非有限数值。"; + return false; + } + if (turnRadians < cornerThresholdRadians) continue; + + int startIndex = cornerIndex - 1; + int endIndex = cornerIndex + 1; + double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength; + if (!NumericGuard.IsPositiveFinite(windowLength)) + { + reason = "Bézier 局部窗口弧长无效。"; + return false; + } + if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue; + if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue; + + var proposed = new Window(startIndex, endIndex); + if (windows.Count == 0 || proposed.StartIndex > windows[windows.Count - 1].EndIndex + 1) + { + windows.Add(proposed); + } + else + { + Window previous = windows[windows.Count - 1]; + windows[windows.Count - 1] = new Window( + previous.StartIndex, + Math.Max(previous.EndIndex, proposed.EndIndex)); + } + } + return true; + } + + private static bool TryAppendWindowInterior( + IReadOnlyList anchors, + Window window, + double handleLengthRatio, + double strength, + double reserveMeters, + List output, + CancellationToken cancellationToken, + out string reason, + out SmoothingCandidateStatus status) + { + reason = string.Empty; + status = SmoothingCandidateStatus.Failed; + SmoothingPoint2D p0 = anchors[window.StartIndex]; + SmoothingPoint2D p3 = anchors[window.EndIndex]; + if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) || + !TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent)) + { + reason = "Bézier 窗口端点包含无效行进切向。"; + return false; + } + + double arcLength = p3.ArcLength - p0.ArcLength; + double handleLength = arcLength * handleLengthRatio * strength; + if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(handleLength)) + { + reason = "Bézier 窗口弧长或控制柄长度无效。"; + return false; + } + + Point2D control1 = new Point2D( + p0.X + entryTangent.X * handleLength, + p0.Y + entryTangent.Y * handleLength); + Point2D control2 = new Point2D( + p3.X - exitTangent.X * handleLength, + p3.Y - exitTangent.Y * handleLength); + if (!IsFinite(control1) || !IsFinite(control2)) + { + reason = "Bézier 控制点产生非有限数值。"; + return false; + } + + for (int index = window.StartIndex + 1; index < window.EndIndex; index++) + { + cancellationToken.ThrowIfCancellationRequested(); + SmoothingPoint2D anchor = anchors[index]; + double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength; + if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d) + { + reason = "Bézier 窗口参数无效。"; + return false; + } + + Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter); + if (!IsFinite(evaluated)) + { + reason = "Bézier 评估产生非有限数值。"; + return false; + } + + double referenceArcLength = p0.ArcLength + parameter * arcLength; + if (!PathReferenceInterpolator.TryInterpolateByArcLength( + anchors, + referenceArcLength, + out SmoothingPoint2D reference, + out reason)) + { + return false; + } + + double displacement = Distance(evaluated, reference); + if (!NumericGuard.IsFinite(displacement)) + { + reason = "Bézier 评估点位移产生非有限数值。"; + return false; + } + double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters); + if (displacement > allowedDisplacement) + { + reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。"; + status = SmoothingCandidateStatus.RetryableInfeasible; + return false; + } + + output.Add(new SmoothingPoint2D( + evaluated.X, + evaluated.Y, + reference.ArcLength, + reference.Heading, + reference.UnwrappedHeading, + reference.BodyClearance, + false, + SmoothedPathPointSource.Interpolated)); + } + return true; + } + + private static bool ContainsGearSwitch(IReadOnlyList anchors, int startIndex, int endIndex) + { + for (int index = startIndex; index <= endIndex; index++) + { + if (anchors[index].IsGearSwitchPoint) return true; + } + return false; + } + + private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent) + { + tangent = default; + double deltaX = end.X - start.X; + double deltaY = end.Y - start.Y; + double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + if (!NumericGuard.IsPositiveFinite(length)) return false; + tangent = new Point2D(deltaX / length, deltaY / length); + return IsFinite(tangent); + } + + private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter) + { + double oneMinusParameter = 1d - parameter; + double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter; + double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter; + double p2Weight = 3d * oneMinusParameter * parameter * parameter; + double p3Weight = parameter * parameter * parameter; + return new Point2D( + p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X, + p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y); + } + + private static bool IsFinite(Point2D point) + { + return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y); + } + + 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 Window + { + internal Window(int startIndex, int endIndex) + { + StartIndex = startIndex; + EndIndex = endIndex; + } + + internal int StartIndex { get; } + + internal int EndIndex { get; } + } + + private readonly struct Point2D + { + internal Point2D(double x, double y) + { + X = x; + Y = y; + } + + internal double X { get; } + + internal double Y { get; } + } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 b/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 new file mode 100644 index 0000000..24fa391 --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 @@ -0,0 +1,278 @@ +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 Assert-PointBitwiseEqual($Expected, $Actual, [string]$Message) { + foreach ($name in @('X', 'Y', 'ArcLength', 'Heading', 'UnwrappedHeading', 'BodyClearance')) { + $expectedBits = [BitConverter]::DoubleToInt64Bits([double]$Expected.$name) + $actualBits = [BitConverter]::DoubleToInt64Bits([double]$Actual.$name) + Assert-Equal $expectedBits $actualBits ($Message + ' ' + $name) + } + Assert-Equal $Expected.IsGearSwitchPoint $Actual.IsGearSwitchPoint ($Message + ' IsGearSwitchPoint') + Assert-Equal $Expected.Source.ToString() $Actual.Source.ToString() ($Message + ' Source') +} + +function New-Point( + [double]$X, + [double]$Y, + [double]$ArcLength, + [double]$Heading, + [double]$BodyClearance = 0.10, + [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) 'Bézier test must create an explicit empty planning map.' + return $map +} + +function New-AlgorithmInput( + [object[]]$Segments, + [double]$ReserveMeters = 0.02, + [double]$CornerThresholdRadians = ([Math]::PI / 18.0), + [double]$MaximumWindowLengthMeters = 0.60, + [double]$HandleLengthRatio = (1.0 / 3.0)) { + $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.LocalCubicBezier.CornerHeadingThresholdRadians = $CornerThresholdRadians + $configuration.LocalCubicBezier.MaximumWindowLengthMeters = $MaximumWindowLengthMeters + $configuration.LocalCubicBezier.HandleLengthRatio = $HandleLengthRatio + $options = $optionsConstructor.Invoke(@($configuration)) + return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options)) +} + +function Invoke-Candidate( + [object[]]$Segments, + [double]$ReserveMeters = 0.02, + [double]$CornerThresholdRadians = ([Math]::PI / 18.0), + [double]$MaximumWindowLengthMeters = 0.60, + [double]$HandleLengthRatio = (1.0 / 3.0), + [double]$Strength = 1.0) { + return $smoothMethod.Invoke($smoother, @( + (New-AlgorithmInput $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio), + $Strength, + [Threading.CancellationToken]::None)) +} + +function Invoke-Smoothing( + [object[]]$Segments, + [double]$ReserveMeters = 0.02, + [double]$CornerThresholdRadians = ([Math]::PI / 18.0), + [double]$MaximumWindowLengthMeters = 0.60, + [double]$HandleLengthRatio = (1.0 / 3.0), + [double]$Strength = 1.0) { + $candidate = Invoke-Candidate $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio $Strength + Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Bézier smoothing must produce a candidate for the deterministic fixture.' + return @(Get-PropertyValue $candidate 'Segments') +} + +function Get-InterpolatedRunCount($Points) { + $runCount = 0 + $inRun = $false + foreach ($point in $Points) { + $interpolated = $point.Source.ToString() -eq 'Interpolated' + if ($interpolated -and -not $inRun) { $runCount++ } + $inRun = $interpolated + } + return $runCount +} + +function Get-PointAtArcLength($Points, [double]$ArcLength) { + foreach ($point in $Points) { + if ([BitConverter]::DoubleToInt64Bits([double]$point.ArcLength) -eq + [BitConverter]::DoubleToInt64Bits($ArcLength)) { + return $point + } + } + throw "No output point found at local arc length $ArcLength." +} + +function Get-FirstInterpolatedPoint($Points) { + foreach ($point in $Points) { + if ($point.Source.ToString() -eq 'Interpolated') { return $point } + } + throw 'Expected an interpolated Bézier point.' +} + +$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.' +$processing = $root + 'Processing.' +$algorithms = $root + 'Algorithms.' +$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.' + +$smootherType = Get-RequiredType ($algorithms + 'LocalCubicBezierSmoother') +$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') +$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters') +$directionType = Get-RequiredType ($coarsePath + 'TravelDirection') +$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource') +$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus') +$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) 'Bézier tests must construct algorithm input with immutable option values.' +$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null) +Assert-True ($null -ne $optionsConstructor) 'Bézier tests must create immutable option snapshots.' +$smoother = [Activator]::CreateInstance($smootherType, $true) +$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public') +Assert-True ($null -ne $smoothMethod) 'LocalCubicBezierSmoother must implement the internal smoother contract.' +Assert-Equal 'LocalCubicBezier' $smoother.Method.ToString() 'Bézier smoother must identify its public smoothing method.' + +$forward = [Enum]::Parse($directionType, 'Forward') +$reverse = [Enum]::Parse($directionType, 'Reverse') +$anchor = [Enum]::Parse($sourceType, 'Anchor') + +# A straight path must not create a local Bézier window or alter any sample. +$straightSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 0.1 0.0 0.1 0.0), + (New-Point 0.2 0.0 0.2 0.0), + (New-Point 0.3 0.0 0.3 0.0)) +$straightOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)))[0].Points +Assert-Equal 0 (Get-InterpolatedRunCount $straightOutput) 'A straight line must create no Bézier replacement window.' +Assert-Equal $straightSource.Count $straightOutput.Count 'A straight line must retain its original sample count.' +for ($index = 0; $index -lt $straightSource.Count; $index++) { + Assert-PointBitwiseEqual $straightSource[$index] $straightOutput[$index] 'Straight samples must remain bitwise unchanged.' +} + +# One corner is one local replacement: only the corner sample is evaluated while the window endpoints stay fixed. +$cornerSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 0.1 0.0 0.1 0.0), + (New-Point 0.2 0.0 0.2 0.0), + (New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)), + (New-Point 0.2 0.2 0.4 ([Math]::PI / 2.0))) +$cornerOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)))[0].Points +Assert-Equal 1 (Get-InterpolatedRunCount $cornerOutput) 'One corner must produce exactly one contiguous Bézier replacement.' +Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must preserve the segment sampling topology.' +Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.' +Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.' +Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.' + +# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits. +$overlappingSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 0.1 0.0 0.1 0.0), + (New-Point 0.2 0.0 0.2 0.0), + (New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)), + (New-Point 0.3 0.1 0.4 0.0), + (New-Point 0.4 0.1 0.5 0.0)) +$overlappingOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)))[0].Points +Assert-Equal 1 (Get-InterpolatedRunCount $overlappingOutput) 'Touching local corner windows must merge into exactly one Bézier replacement.' +Assert-PointBitwiseEqual $overlappingSource[1] $overlappingOutput[1] 'Merged Bézier entry anchor must remain fixed.' +Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bézier exit anchor must remain fixed.' +Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.' +Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.' + +# Samples outside a local window must remain bitwise unchanged rather than be globally re-fit. +$isolatedSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 0.1 0.0 0.1 0.0), + (New-Point 0.2 0.0 0.2 0.0), + (New-Point 0.3 0.0 0.3 0.0), + (New-Point 0.3 0.1 0.4 ([Math]::PI / 2.0)), + (New-Point 0.3 0.2 0.5 ([Math]::PI / 2.0)), + (New-Point 0.3 0.3 0.6 ([Math]::PI / 2.0))) +$isolatedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $isolatedSource)))[0].Points +foreach ($index in @(0, 1, 2, 5, 6)) { + Assert-PointBitwiseEqual $isolatedSource[$index] (Get-PointAtArcLength $isolatedOutput $isolatedSource[$index].ArcLength) 'Samples outside a Bézier window must remain bitwise unchanged.' +} + +# Direction segments stay independent; segment endpoints and the gear-switch anchor are fixed. +$reverseSource = @( + (New-Point 0.2 0.2 0.0 ([Math]::PI / 2.0) 0.10 $true), + (New-Point 0.2 0.1 0.1 ([Math]::PI / 2.0)), + (New-Point 0.2 0.0 0.2 ([Math]::PI / 2.0))) +$switchOutput = @(Invoke-Smoothing @( + (New-DirectionSegment 0 $forward $cornerSource $false $true), + (New-DirectionSegment 1 $reverse $reverseSource $true $false))) +Assert-Equal 2 $switchOutput.Count 'Bézier smoothing must retain separate forward and reverse direction segments.' +Assert-True $switchOutput[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.' +Assert-True $switchOutput[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.' +Assert-PointBitwiseEqual $cornerSource[0] $switchOutput[0].Points[0] 'Segment start endpoint must remain fixed.' +Assert-PointBitwiseEqual $cornerSource[$cornerSource.Count - 1] $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] 'Segment end endpoint must remain fixed.' +Assert-PointBitwiseEqual $reverseSource[0] $switchOutput[1].Points[0] 'Gear-switch point must remain fixed.' + +# The immutable options each change only their own local behavior. +$thresholdOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.70)[0].Points +Assert-Equal 0 (Get-InterpolatedRunCount $thresholdOutput) 'A non-default heading threshold above the corner angle must suppress only corner detection.' +$windowOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.15)[0].Points +Assert-Equal 0 (Get-InterpolatedRunCount $windowOutput) 'A non-default maximum window shorter than the local connection must suppress only that window.' +$shortHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.10)[0].Points +$longHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.60)[0].Points +Assert-Equal 1 (Get-InterpolatedRunCount $shortHandleOutput) 'Changing handle ratio must not change detected window topology.' +Assert-Equal 1 (Get-InterpolatedRunCount $longHandleOutput) 'Changing handle ratio must not change detected window topology.' +$shortHandlePoint = Get-FirstInterpolatedPoint $shortHandleOutput +$longHandlePoint = Get-FirstInterpolatedPoint $longHandleOutput +Assert-True (($shortHandlePoint.X -ne $longHandlePoint.X) -or ($shortHandlePoint.Y -ne $longHandlePoint.Y)) 'A non-default handle ratio must change only the local Bézier geometry.' + +# Parameter-matched local arc-length reference comparison must reject excess displacement as retryable and publish no geometry. +$infeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.095 +Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToString() 'Exceeded local arc-length displacement must be retryable, not terminal.' +Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.' +Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.' + +Write-Output 'Path smoothing local cubic Bézier checks passed.'