fix: align smoothing feasibility and option flow
This commit is contained in:
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user