feat: add piecewise quintic path smoother

2026-07-29 13:55:56 +08:00
parent 591a10cc3d
commit b13f9f0163
2 changed files with 845 additions and 0 deletions
@@ -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.'