fix: align smoothing feasibility and option flow

This commit is contained in:
梁薄云
2026-07-29 12:39:46 +08:00
parent fc9aff4d84
commit d670a9c821
9 changed files with 466 additions and 102 deletions
@@ -62,7 +62,10 @@ function New-EmptyMap {
return $map
}
function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
function New-AlgorithmInput(
[object[]]$Segments,
[double]$ReserveMeters,
[double]$EndpointTangentScale = (1.0 / 3.0)) {
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
for ($index = 0; $index -lt $Segments.Count; $index++) {
$typedSegments.SetValue($Segments[$index], $index)
@@ -74,16 +77,27 @@ function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
$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))
$configuration = [Activator]::CreateInstance($configurationType)
$configuration.CubicBSpline.EndpointTangentScale = $EndpointTangentScale
$options = $optionsConstructor.Invoke(@($configuration))
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
}
function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
function Invoke-Candidate(
[object[]]$Segments,
[double]$ReserveMeters,
[double]$Strength = 1.0,
[double]$EndpointTangentScale = (1.0 / 3.0)) {
return $smoothMethod.Invoke($smoother, @(
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
(New-AlgorithmInput $Segments $ReserveMeters $EndpointTangentScale), $Strength, [Threading.CancellationToken]::None))
}
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength
function Invoke-Smoothing(
[object[]]$Segments,
[double]$ReserveMeters,
[double]$Strength = 1.0,
[double]$EndpointTangentScale = (1.0 / 3.0)) {
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength $EndpointTangentScale
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
return @(Get-PropertyValue $candidate 'Segments')
}
@@ -137,6 +151,9 @@ $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')
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
@@ -146,8 +163,12 @@ $mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.Plann
$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.'
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable smoothing options and the minimum clearance reserve for per-anchor movement limits.'
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
Assert-True ($null -ne $optionsConstructor) 'B-spline tests must create an immutable options snapshot.'
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose arc-length interpolation.'
$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.'
@@ -157,6 +178,20 @@ $forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anchor = [Enum]::Parse($sourceType, 'Anchor')
# Arc-length interpolation must bracket by local arc rather than by sample index.
$nonUniformPoints = [Array]::CreateInstance($pointType, 4)
$nonUniformPoints.SetValue((New-Point 0.0 0.0 0.000 0.0 1.0), 0)
$nonUniformPoints.SetValue((New-Point 5.0 0.0 0.050 0.1 0.9), 1)
$nonUniformPoints.SetValue((New-Point 10.0 0.0 0.100 0.2 0.8), 2)
$nonUniformPoints.SetValue((New-Point 20.0 0.0 0.125 0.3 0.7), 3)
$interpolateArguments = [object[]]@($nonUniformPoints, [double]0.1125, $null, $null)
Assert-True $interpolateMethod.Invoke($null, $interpolateArguments) 'Arc-length interpolation must accept a target within the final non-uniform interval.'
$arcReference = $interpolateArguments[2]
Assert-Near 15.0 $arcReference.X 0.000000001 'Target arc length 0.1125 must lie halfway through the final 0.100-0.125 interval, independent of point count.'
Assert-Near 0.1125 $arcReference.ArcLength 0.000000001 'Arc-length interpolation must preserve the requested target arc length.'
Assert-Near 0.25 $arcReference.Heading 0.000000001 'Arc-length interpolation must linearly interpolate heading.'
Assert-Near 0.75 $arcReference.BodyClearance 0.000000001 'Arc-length interpolation must linearly interpolate clearance.'
# 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),
@@ -196,6 +231,11 @@ for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
}
# Endpoint tangent scale is an immutable option and must influence the clamped B-spline endpoint handle.
$shortHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.20)[0].Points
$longHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.60)[0].Points
Assert-True (($longHandlePoints[2].X - $shortHandlePoints[2].X) -gt 0.0001) 'A custom endpoint tangent scale must change the B-spline start handle and early curve samples.'
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
$allowedRadius = 0.06
foreach ($point in $cornerPoints) {