From 24b46732e3a6a2057d93cb33949f34b8489f55f2 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 10:45:11 +0800 Subject: [PATCH] feat: add cubic b-spline path smoother --- .../Algorithms/CubicBSplineSmoother.cs | 322 ++++++++++++++++++ .../Algorithms/SmoothingAlgorithmInput.cs | 7 +- .../Algorithms/SmoothingAlgorithmRunner.cs | 2 +- .../tests/verify_path_smoothing_bspline.ps1 | 217 ++++++++++++ 4 files changed, 546 insertions(+), 2 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs create mode 100644 ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs new file mode 100644 index 0000000..168ac04 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs @@ -0,0 +1,322 @@ +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; + +/// 按方向段独立生成夹持三次 B 样条原始几何候选。 +internal sealed class CubicBSplineSmoother : IPathSmoother +{ + private const int Degree = 3; + private const int SamplesPerSpan = 64; + private const double StraightToleranceMeters = 1e-9d; + private const double EndpointTangentScale = 1d / 3d; + private const double EndpointProbeParameter = 1e-6d; + + /// + public SmoothingMethod Method => SmoothingMethod.CubicBSpline; + + /// + public SmoothingCandidate Smooth( + SmoothingAlgorithmInput input, + double effectiveStrength, + CancellationToken cancellationToken) + { + if (input == null || input.OriginalPath == null || + !NumericGuard.IsPositiveFinite(effectiveStrength) || + !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) || + input.MinimumClearanceReserveMeters < 0d) + { + return SmoothingCandidate.Failed("B 样条输入、强度或净空预留无效。"); + } + + 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, + cancellationToken, out IReadOnlyList points, out string reason)) + { + return 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 strength, + double reserveMeters, + CancellationToken cancellationToken, + out IReadOnlyList result, + out string reason) + { + result = null; + reason = string.Empty; + if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0) + { + reason = "B 样条方向段为空。"; + return false; + } + + IReadOnlyList anchors = sourceSegment.Points; + for (int index = 0; index < anchors.Count; index++) + { + cancellationToken.ThrowIfCancellationRequested(); + if (!IsValidAnchor(anchors[index])) + { + reason = "B 样条方向段包含非法锚点。"; + return false; + } + } + + if (anchors.Count <= Degree || IsStraight(anchors)) + { + result = anchors; + return true; + } + + Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, + cancellationToken); + if (controls == null) + { + reason = "B 样条控制点构造产生非法数值。"; + return false; + } + + double[] knots = CreateClampedKnots(controls.Length); + var sampled = new List(); + int spanCount = controls.Length - Degree; + int uniformIntervals = spanCount * SamplesPerSpan; + AddSample(0d, anchors, controls, knots, sampled, cancellationToken); + AddSample(EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken); + for (int index = 1; index < uniformIntervals; index++) + { + AddSample((double)index / uniformIntervals, anchors, controls, knots, sampled, cancellationToken); + } + AddSample(1d - EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken); + AddSample(1d, anchors, controls, knots, sampled, cancellationToken); + + result = sampled; + return true; + } + + private static Point2D[] CreateControls( + IReadOnlyList anchors, + TravelDirection direction, + double strength, + double reserveMeters, + CancellationToken cancellationToken) + { + var controls = new Point2D[anchors.Count]; + controls[0] = Point2D.FromAnchor(anchors[0]); + controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]); + + double startHandleLength = Distance(anchors[0], anchors[1]) * EndpointTangentScale * strength; + double startTravelHeading = GetTravelHeading(anchors[0], direction); + Point2D startProposed = new Point2D( + anchors[0].X + startHandleLength * Math.Cos(startTravelHeading), + anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading)); + controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters)); + + int finalIndex = anchors.Count - 1; + double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength; + double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction); + Point2D endProposed = new Point2D( + anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading), + anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading)); + controls[finalIndex - 1] = ClampDisplacement( + anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters)); + + for (int index = 2; index < finalIndex - 1; index++) + { + cancellationToken.ThrowIfCancellationRequested(); + SmoothingPoint2D previous = anchors[index - 1]; + SmoothingPoint2D current = anchors[index]; + SmoothingPoint2D next = anchors[index + 1]; + Point2D target = new Point2D( + (previous.X + current.X + next.X) / 3d, + (previous.Y + current.Y + next.Y) / 3d); + Point2D proposed = new Point2D( + current.X + strength * (target.X - current.X), + current.Y + strength * (target.Y - current.Y)); + controls[index] = ClampDisplacement(current, proposed, GetAllowedRadius(current, reserveMeters)); + } + + for (int index = 0; index < controls.Length; index++) + { + if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y)) + return null; + } + return controls; + } + + private static void AddSample( + double parameter, + IReadOnlyList anchors, + Point2D[] controls, + double[] knots, + List output, + CancellationToken cancellationToken) + { + cancellationToken.ThrowIfCancellationRequested(); + Point2D evaluated = Evaluate(controls, knots, parameter); + SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter); + bool endpoint = parameter == 0d || parameter == 1d; + output.Add(new SmoothingPoint2D( + evaluated.X, + evaluated.Y, + reference.ArcLength, + reference.Heading, + reference.UnwrappedHeading, + reference.BodyClearance, + endpoint && reference.IsGearSwitchPoint, + endpoint ? reference.Source : SmoothedPathPointSource.Interpolated)); + } + + private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter) + { + if (parameter <= 0d) return controls[0]; + if (parameter >= 1d) return controls[controls.Length - 1]; + + var point = new Point2D(0d, 0d); + for (int index = 0; index < controls.Length; index++) + { + double basis = EvaluateBasis(index, Degree, parameter, knots); + point = new Point2D(point.X + basis * controls[index].X, point.Y + basis * controls[index].Y); + } + return point; + } + + private static double EvaluateBasis(int index, int degree, double parameter, double[] knots) + { + if (degree == 0) + return knots[index] <= parameter && parameter < knots[index + 1] ? 1d : 0d; + + double left = 0d; + double leftDenominator = knots[index + degree] - knots[index]; + if (leftDenominator > 0d) + left = (parameter - knots[index]) / leftDenominator * EvaluateBasis(index, degree - 1, parameter, knots); + + double right = 0d; + double rightDenominator = knots[index + degree + 1] - knots[index + 1]; + if (rightDenominator > 0d) + right = (knots[index + degree + 1] - parameter) / rightDenominator * + EvaluateBasis(index + 1, degree - 1, parameter, knots); + return left + right; + } + + private static double[] CreateClampedKnots(int controlCount) + { + var knots = new double[controlCount + Degree + 1]; + for (int index = Degree + 1; index < controlCount; index++) + knots[index] = (double)(index - Degree) / (controlCount - Degree); + for (int index = controlCount; index < knots.Length; index++) knots[index] = 1d; + return knots; + } + + private static SmoothingPoint2D InterpolateAnchor(IReadOnlyList anchors, double parameter) + { + if (parameter <= 0d) return anchors[0]; + if (parameter >= 1d) return anchors[anchors.Count - 1]; + + double scaled = parameter * (anchors.Count - 1); + int leftIndex = (int)Math.Floor(scaled); + double ratio = scaled - leftIndex; + SmoothingPoint2D left = anchors[leftIndex]; + SmoothingPoint2D right = anchors[leftIndex + 1]; + return new SmoothingPoint2D( + left.X + ratio * (right.X - left.X), + left.Y + ratio * (right.Y - left.Y), + left.ArcLength + ratio * (right.ArcLength - left.ArcLength), + left.Heading + ratio * (right.Heading - left.Heading), + left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading), + left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance), + false, + SmoothedPathPointSource.Interpolated); + } + + private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius) + { + double deltaX = proposed.X - anchor.X; + double deltaY = proposed.Y - anchor.Y; + double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + if (!NumericGuard.IsFinite(distance) || distance <= allowedRadius) return proposed; + if (distance == 0d || allowedRadius == 0d) return Point2D.FromAnchor(anchor); + double scale = allowedRadius / distance; + return new Point2D(anchor.X + deltaX * scale, anchor.Y + deltaY * scale); + } + + private static bool IsStraight(IReadOnlyList anchors) + { + if (anchors.Count < 3) return true; + SmoothingPoint2D first = anchors[0]; + SmoothingPoint2D last = anchors[anchors.Count - 1]; + double directionX = last.X - first.X; + double directionY = last.Y - first.Y; + double length = Math.Sqrt(directionX * directionX + directionY * directionY); + if (!NumericGuard.IsFinite(length) || length <= StraightToleranceMeters) return false; + for (int index = 1; index < anchors.Count - 1; index++) + { + double offsetX = anchors[index].X - first.X; + double offsetY = anchors[index].Y - first.Y; + double perpendicularDeviation = Math.Abs(directionX * offsetY - directionY * offsetX) / length; + if (perpendicularDeviation >= StraightToleranceMeters) return false; + } + return true; + } + + private static bool IsValidAnchor(SmoothingPoint2D point) + { + return 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.BodyClearance >= 0d; + } + + private static double GetAllowedRadius(SmoothingPoint2D anchor, double reserveMeters) + { + return Math.Max(0d, anchor.BodyClearance - reserveMeters); + } + + private static double GetTravelHeading(SmoothingPoint2D point, TravelDirection direction) + { + return direction == TravelDirection.Forward ? point.Heading : point.Heading - Math.PI; + } + + private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) + { + double deltaX = right.X - left.X; + double deltaY = right.Y - left.Y; + return Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + } + + private readonly struct Point2D + { + internal Point2D(double x, double y) + { + X = x; + Y = y; + } + + internal double X { get; } + internal double Y { get; } + + internal static Point2D FromAnchor(SmoothingPoint2D anchor) + { + return new Point2D(anchor.X, anchor.Y); + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs index 87c52cf..05dd810 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs @@ -12,12 +12,14 @@ internal sealed class SmoothingAlgorithmInput PreparedPath originalPath, PlanningGridMap map, VehicleParameters vehicle, - double maximumCollisionCheckStepMeters) + double maximumCollisionCheckStepMeters, + double minimumClearanceReserveMeters) { OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath)); Map = map ?? throw new ArgumentNullException(nameof(map)); Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle)); MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters; + MinimumClearanceReserveMeters = minimumClearanceReserveMeters; } /// 已校验并按方向分段的原始路径。 @@ -31,4 +33,7 @@ internal sealed class SmoothingAlgorithmInput /// 连续车体碰撞检查的最大步长，单位 m。 internal double MaximumCollisionCheckStepMeters { get; } + + /// 候选几何必须从原始保守净空中预留的最小安全余量，单位 m。 + internal double MinimumClearanceReserveMeters { get; } } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs index 9f24798..c70934b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs @@ -277,7 +277,7 @@ internal sealed class SmoothingAlgorithmRunner MaximumCurvaturePerMeter = 100d, MinimumTurningRadiusMeters = 0.01d, }; - return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d); + return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d, 0.02d); } private static SmoothingCandidate CreateAcceptedCandidate() diff --git a/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 b/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 new file mode 100644 index 0000000..ee5395d --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 @@ -0,0 +1,217 @@ +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, + [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-spline test must create an explicit empty planning map.' + return $map +} + +function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) { + $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 + return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters)) +} + +function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) { + $candidate = $smoothMethod.Invoke($smoother, @( + (New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None)) + Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline 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-DistanceToSegment($Point, $Left, $Right) { + $deltaX = $Right.X - $Left.X + $deltaY = $Right.Y - $Left.Y + $lengthSquared = $deltaX * $deltaX + $deltaY * $deltaY + if ($lengthSquared -le 0.0) { return Get-PointDistance $Point $Left } + $projection = (($Point.X - $Left.X) * $deltaX + ($Point.Y - $Left.Y) * $deltaY) / $lengthSquared + $projection = [Math]::Max(0.0, [Math]::Min(1.0, $projection)) + $closest = New-Object PSObject -Property @{ + X = $Left.X + $projection * $deltaX + Y = $Left.Y + $projection * $deltaY + } + return Get-PointDistance $Point $closest +} + +function Get-DistanceToPolyline($Point, [object[]]$SourcePoints) { + $minimum = [double]::PositiveInfinity + for ($index = 1; $index -lt $SourcePoints.Count; $index++) { + $minimum = [Math]::Min($minimum, (Get-DistanceToSegment $Point $SourcePoints[$index - 1] $SourcePoints[$index])) + } + return $minimum +} + +function Get-TravelAngle($Left, $Right) { + return [Math]::Atan2($Right.Y - $Left.Y, $Right.X - $Left.X) +} + +function Get-AngleDifference([double]$Left, [double]$Right) { + $difference = $Left - $Right + while ($difference -gt [Math]::PI) { $difference -= 2.0 * [Math]::PI } + while ($difference -lt -[Math]::PI) { $difference += 2.0 * [Math]::PI } + return [Math]::Abs($difference) +} + +$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.' +$processing = $root + 'Processing.' +$algorithms = $root + 'Algorithms.' +$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.' + +$smootherType = Get-RequiredType ($algorithms + 'CubicBSplineSmoother') +$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D') +$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment') +$preparedPathType = Get-RequiredType ($processing + 'PreparedPath') +$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput') +$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]), $null) +Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry the minimum clearance reserve for per-anchor movement limits.' +$smoother = [Activator]::CreateInstance($smootherType, $true) +$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public') +Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.' +Assert-Equal 'CubicBSpline' $smoother.Method.ToString() 'B-spline smoother must identify its public smoothing method.' + +$forward = [Enum]::Parse($directionType, 'Forward') +$reverse = [Enum]::Parse($directionType, 'Reverse') +$anchor = [Enum]::Parse($sourceType, 'Anchor') + +# Straight samples are returned exactly, so a straight is never distorted or densified. +$straightSource = @( + (New-Point 0.0 0.0 0.0 0.0 0.03), + (New-Point 1.0 0.0 1.0 0.0 0.03), + (New-Point 2.0 0.0 2.0 0.0 0.03), + (New-Point 3.0 0.0 3.0 0.0 0.03)) +$straightResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)) 0.02 +Assert-Equal 1 $straightResult.Count 'A single direction segment must produce exactly one candidate segment.' +Assert-Equal $straightSource.Count $straightResult[0].Points.Count 'A straight must retain its original samples.' +for ($index = 0; $index -lt $straightSource.Count; $index++) { + Assert-Near $straightSource[$index].X $straightResult[0].Points[$index].X 0.0 'Straight X coordinates must remain exact.' + Assert-Near $straightSource[$index].Y $straightResult[0].Points[$index].Y 0.0 'Straight Y coordinates must remain exact.' +} + +# A five-anchor corner uses the preprocessor's 0.05 m sampling scale. It must retain exact endpoint poses, +# follow endpoint travel tangents, turn continuously, and stay within the per-anchor clearance reserve radius. +$cornerSource = @( + (New-Point 0.0 0.0 0.0 0.0 0.08), + (New-Point 0.05 0.0 0.05 0.0 0.08), + (New-Point 0.10 0.0 0.10 0.0 0.08), + (New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08), + (New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08)) +$cornerResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 +$cornerPoints = @($cornerResult[0].Points) +Assert-True ($cornerPoints.Count -gt $cornerSource.Count) 'A non-straight B-spline candidate must provide sampled curve geometry.' +$cornerStart = $cornerPoints[0] +$cornerEnd = $cornerPoints[$cornerPoints.Count - 1] +Assert-Near $cornerSource[0].X $cornerStart.X 0.0 'B-spline start X must be exact.' +Assert-Near $cornerSource[0].Y $cornerStart.Y 0.0 'B-spline start Y must be exact.' +Assert-Near $cornerSource[$cornerSource.Count - 1].X $cornerEnd.X 0.0 'B-spline end X must be exact.' +Assert-Near $cornerSource[$cornerSource.Count - 1].Y $cornerEnd.Y 0.0 'B-spline end Y must be exact.' +Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[0] $cornerPoints[1]) 0.0) 0.02 'B-spline start travel tangent must follow the supplied forward heading.' +Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[$cornerPoints.Count - 2] $cornerPoints[$cornerPoints.Count - 1]) ([Math]::PI / 2.0)) 0.02 'B-spline end travel tangent must follow the supplied forward heading.' +for ($index = 2; $index -lt $cornerPoints.Count; $index++) { + $previousAngle = Get-TravelAngle $cornerPoints[$index - 2] $cornerPoints[$index - 1] + $currentAngle = Get-TravelAngle $cornerPoints[$index - 1] $cornerPoints[$index] + Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.' +} + +# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance. +$allowedRadius = 0.06 +foreach ($point in $cornerPoints) { + Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.' +} + +# Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch. +$reverseSource = @( + (New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true), + (New-Point 0.10 0.05 0.05 ([Math]::PI / 2.0) 0.08), + (New-Point 0.10 0.0 0.10 ([Math]::PI / 2.0) 0.08)) +$switchResult = Invoke-Smoothing @( + (New-DirectionSegment 0 $forward $cornerSource $false $true), + (New-DirectionSegment 1 $reverse $reverseSource $true $false)) 0.02 +Assert-Equal 2 $switchResult.Count 'B-spline smoothing must preserve each direction segment boundary.' +Assert-True $switchResult[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.' +Assert-True $switchResult[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.' +$switchLeft = $switchResult[0].Points[$switchResult[0].Points.Count - 1] +$switchRight = $switchResult[1].Points[0] +Assert-Near $switchLeft.X $switchRight.X 0.0 'B-spline smoothing must retain the duplicated gear-switch X pose.' +Assert-Near $switchLeft.Y $switchRight.Y 0.0 'B-spline smoothing must retain the duplicated gear-switch Y pose.' + +Write-Output 'Path smoothing cubic B-spline checks passed.'