fix: enforce b-spline displacement constraints

This commit is contained in:
梁薄云
2026-07-29 11:01:28 +08:00
parent 24b46732e3
commit 5e6e068165
2 changed files with 129 additions and 27 deletions
@@ -77,9 +77,13 @@ function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
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, @(
function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
return $smoothMethod.Invoke($smoother, @(
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
}
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
return @(Get-PropertyValue $candidate 'Segments')
}
@@ -198,6 +202,26 @@ 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.'
}
# A tight reserve may not publish an evaluated B-spline that leaves its 0.005 m movement radius.
$tightReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.075
# A tight endpoint circle that cannot meet the heading=0.2 rad tangent ray must fail, not silently rotate the handle.
$misalignedHeadingSource = @(
(New-Point 0.0 0.0 0.0 0.2 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))
$misalignedHeadingCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $misalignedHeadingSource)) 0.075
$requiredFailures = New-Object System.Collections.Generic.List[string]
if (Get-PropertyValue $tightReserveCandidate 'Succeeded') {
[void]$requiredFailures.Add('A tight reserve published an evaluated candidate outside its permitted movement radius.')
}
if (Get-PropertyValue $misalignedHeadingCandidate 'Succeeded') {
[void]$requiredFailures.Add('A tight reserve silently accepted an endpoint handle that cannot follow the supplied travel tangent.')
}
Assert-Equal 0 $requiredFailures.Count ([string]::Join(' ', $requiredFailures))
# 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),