feat: build local G2 smoothing candidates

This commit is contained in:
梁薄云
2026-07-30 17:26:06 +08:00
parent a3b6ea7d84
commit 1c433458bc
4 changed files with 819 additions and 0 deletions
@@ -0,0 +1,61 @@
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)
}
$builderType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateBuilder'
$hooksType = $builderType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $hooksType) 'LocalG2CandidateBuilder must expose narrowly scoped deterministic TestHooks.'
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
Assert-True ($null -ne $executeMethod) 'TestHooks must execute deterministic candidate scenarios.'
function Invoke-Scenario([string]$Scenario) {
return $executeMethod.Invoke($null, @($Scenario))
}
$isolated = Invoke-Scenario 'Isolated'
Assert-True ($isolated.CandidateCount -gt 0 -and $isolated.CandidateCount -le 12) `
'An isolated event must produce a bounded non-empty candidate set.'
Assert-Near 0.0 $isolated.StartPositionError 1e-9 'Window start position must match.'
Assert-Near 0.0 $isolated.EndPositionError 1e-9 'Window end position must match.'
Assert-Near 0.0 $isolated.StartCurvatureError 1e-8 'Window start curvature must match.'
Assert-Near 0.0 $isolated.EndCurvatureError 1e-8 'Window end curvature must match.'
Assert-True $isolated.ContainsLocalG2Source 'Generated samples must identify their source.'
$cluster = Invoke-Scenario 'Cluster'
Assert-Equal 1 $cluster.OutputRegionCount 'Overlapping events must be built as one region.'
Assert-True $cluster.InternalConnectionsAreG2 'Internal anchors must share tangent and curvature.'
$reverse = Invoke-Scenario 'Reverse'
Assert-Equal 'Reverse' $reverse.Direction 'Reverse candidates must preserve segment direction.'
Assert-True $reverse.VehicleAndGeometricCurvatureSignsAreOpposite `
'Reverse candidates must convert vehicle curvature to geometric curvature exactly once.'
$spliced = Invoke-Scenario 'Spliced'
Assert-True $spliced.NoDuplicateNonGearPoints 'Splicing must remove duplicate boundary samples.'
Assert-True $spliced.EndpointsUnchanged 'Splicing must preserve the complete segment endpoints.'
$startBoundary = Invoke-Scenario 'StartBoundary'
Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must build a one-sided candidate.'
Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 `
'A start-boundary candidate must use the preserved physical start vehicle curvature.'
Write-Output 'Path smoothing Local G2 candidate checks passed.'