feat: add cubic b-spline path smoother

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;
+
+/// <summary>按方向段独立生成夹持三次 B 样条原始几何候选。</summary>
+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;
+
+    /// <inheritdoc />
+    public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
+
+    /// <inheritdoc />
+    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<PreparedDirectionSegment>(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<SmoothingPoint2D> 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<SmoothingPoint2D> result,
+        out string reason)
+    {
+        result = null;
+        reason = string.Empty;
+        if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
+        {
+            reason = "B 样条方向段为空。";
+            return false;
+        }
+
+        IReadOnlyList<SmoothingPoint2D> 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<SmoothingPoint2D>();
+        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<SmoothingPoint2D> 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<SmoothingPoint2D> anchors,
+        Point2D[] controls,
+        double[] knots,
+        List<SmoothingPoint2D> 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<SmoothingPoint2D> 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<SmoothingPoint2D> 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);
+        }
+    }
+}

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;
     }
 
     /// <summary>已校验并按方向分段的原始路径。</summary>
@@ -31,4 +33,7 @@ internal sealed class SmoothingAlgorithmInput
 
     /// <summary>连续车体碰撞检查的最大步长，单位 m。</summary>
     internal double MaximumCollisionCheckStepMeters { get; }
+
+    /// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
+    internal double MinimumClearanceReserveMeters { get; }
 }

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()

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.'