643 lines
35 KiB
PowerShell
643 lines
35 KiB
PowerShell
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 Assert-False($Actual, [string]$Message) {
|
|
if ($Actual) { throw $Message }
|
|
}
|
|
|
|
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
|
try {
|
|
& $Action
|
|
}
|
|
catch {
|
|
return
|
|
}
|
|
throw $Message
|
|
}
|
|
|
|
function Get-RequiredType([string]$Name) {
|
|
return $assembly.GetType($Name, $true)
|
|
}
|
|
|
|
function New-GeometryPoint(
|
|
[double]$X,
|
|
[double]$Y,
|
|
[double]$ArcLength,
|
|
[double]$Heading,
|
|
[double]$UnwrappedHeading,
|
|
[bool]$IsGearSwitch = $false) {
|
|
return [Activator]::CreateInstance($pointType, @(
|
|
$X, $Y, $ArcLength, $Heading, $UnwrappedHeading,
|
|
[double]1.0, $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-DirectionSegmentWithStartCurvature(
|
|
[int]$Index,
|
|
$Direction,
|
|
[double]$StartVehicleCurvature,
|
|
[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, $StartVehicleCurvature))
|
|
}
|
|
|
|
function New-CoarsePoint(
|
|
[double]$X,
|
|
[double]$Y,
|
|
[double]$ArcLength,
|
|
$Direction,
|
|
[bool]$IsGearSwitch = $false) {
|
|
return [Activator]::CreateInstance($coarsePointType, @(
|
|
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
|
|
[double]0.0, [double]1.0, $IsGearSwitch, $coarseAnchor))
|
|
}
|
|
|
|
function Invoke-Analysis([object[]]$Segments, [double]$Spacing = 0.05) {
|
|
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
|
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
|
$typedSegments.SetValue($Segments[$index], $index)
|
|
}
|
|
|
|
$arguments = [object[]]@($typedSegments, $Spacing, $null, $null)
|
|
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
|
$description = [string]::Join(',', @($Segments | ForEach-Object {
|
|
"index=$($_.SegmentIndex);direction=$($_.Direction);points=$($_.Points.Count)"
|
|
}))
|
|
Assert-True $accepted ("Geometry analysis must accept the analytic candidate. Reason=" + $arguments[3] + '; Segments=' + $description)
|
|
Assert-True ($null -ne $arguments[2]) 'Successful geometry analysis must return PathGeometryAnalysis.'
|
|
return $arguments[2]
|
|
}
|
|
|
|
function Assert-AnalysisRejected([object[]]$Segments, [string]$Message) {
|
|
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
|
for ($segmentIndex = 0; $segmentIndex -lt $Segments.Count; $segmentIndex++) {
|
|
$typedSegments.SetValue($Segments[$segmentIndex], $segmentIndex)
|
|
}
|
|
|
|
$arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
|
|
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
|
Assert-True (-not $accepted) ($Message + '; Reason=' + $arguments[3])
|
|
}
|
|
|
|
function Invoke-RejectedAnalysis([object[]]$Segments, [string]$Message) {
|
|
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
|
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
|
$typedSegments.SetValue($Segments[$index], $index)
|
|
}
|
|
|
|
$arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
|
|
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
|
Assert-False $accepted ($Message + '; Reason=' + $arguments[3])
|
|
}
|
|
|
|
function New-CoarsePathPoint(
|
|
[double]$X,
|
|
[double]$Y,
|
|
[double]$ArcLength,
|
|
$Direction,
|
|
[bool]$IsGearSwitch = $false,
|
|
[string]$SourceName = 'MotionPrimitive',
|
|
[double]$Heading = 0.0,
|
|
[double]$VehicleCurvature = 0.0) {
|
|
return [Activator]::CreateInstance($coarsePointType, @(
|
|
$X, $Y, $Heading, $Heading, $ArcLength,
|
|
$Direction, $VehicleCurvature, [double]1.0, $IsGearSwitch,
|
|
[Enum]::Parse($coarsePointSourceType, $SourceName)))
|
|
}
|
|
|
|
function New-EmptyGeometryMap {
|
|
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
|
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
|
$mapRequest.ResolutionMm = [single]50
|
|
$mapRequest.AllowExplicitEmptyMap = $true
|
|
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
|
Assert-True ($null -ne $map) 'Geometry test must create an explicit empty planning map.'
|
|
return $map
|
|
}
|
|
|
|
function Invoke-GeometryValidation($Analysis, [object[]]$Segments, $Vehicle) {
|
|
$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))
|
|
$arguments = [object[]]@(
|
|
$Analysis.Path, $Analysis.Segments, $preparedPath, (New-EmptyGeometryMap), $Vehicle, [double]0.05,
|
|
$null, [double]0.0, $null)
|
|
$accepted = $validateMethod.Invoke($validator, $arguments)
|
|
return [pscustomobject]@{ Accepted = $accepted; Reason = $arguments[8] }
|
|
}
|
|
|
|
function New-CircularDirectionSegment(
|
|
$Direction,
|
|
[double]$Curvature,
|
|
[int]$Intervals,
|
|
[double]$ChordLength) {
|
|
$radius = 1.0 / $Curvature
|
|
$headingStep = 2.0 * [Math]::Asin($Curvature * $ChordLength / 2.0)
|
|
$points = New-Object System.Collections.Generic.List[object]
|
|
for ($index = 0; $index -le $Intervals; $index++) {
|
|
$theta = $headingStep * $index
|
|
$heading = if ($Direction.ToString() -eq 'Forward') { $theta } else { $theta + [Math]::PI }
|
|
[void]$points.Add((New-GeometryPoint `
|
|
(1.0 + $radius * [Math]::Sin($theta)) `
|
|
(1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
|
|
($index * $ChordLength) $heading $heading))
|
|
}
|
|
return New-DirectionSegment 0 $Direction $points.ToArray()
|
|
}
|
|
|
|
function Get-OldPolylineCurvatureMaximum($Analysis) {
|
|
$maximum = 0.0
|
|
$path = $Analysis.Path
|
|
for ($index = 0; $index -lt $path.Count; $index++) {
|
|
if ($path.Count -eq 1) {
|
|
$curvature = 0.0
|
|
}
|
|
elseif ($index -eq 0) {
|
|
$curvature = ($path[1].UnwrappedHeading - $path[0].UnwrappedHeading) /
|
|
($path[1].ArcLength - $path[0].ArcLength)
|
|
}
|
|
elseif ($index -eq $path.Count - 1) {
|
|
$curvature = ($path[$index].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
|
|
($path[$index].ArcLength - $path[$index - 1].ArcLength)
|
|
}
|
|
else {
|
|
$curvature = ($path[$index + 1].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
|
|
($path[$index + 1].ArcLength - $path[$index - 1].ArcLength)
|
|
}
|
|
$maximum = [Math]::Max($maximum, [Math]::Abs($curvature))
|
|
}
|
|
return $maximum
|
|
}
|
|
|
|
function Get-QuinticAnalyticCurvatureMaximum(
|
|
[double]$C2,
|
|
[double]$C3,
|
|
[double]$C4,
|
|
[double]$C5,
|
|
[int]$ReferenceSamples = 20000) {
|
|
$maximum = 0.0
|
|
for ($index = 0; $index -lt $ReferenceSamples; $index++) {
|
|
$t = $index / [double]($ReferenceSamples - 1)
|
|
$firstDerivative = 2.0 * $C2 * $t + 3.0 * $C3 * $t * $t +
|
|
4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t
|
|
$secondDerivative = 2.0 * $C2 + 6.0 * $C3 * $t +
|
|
12.0 * $C4 * $t * $t + 20.0 * $C5 * $t * $t * $t
|
|
$curvature = [Math]::Abs($secondDerivative / [Math]::Pow(1.0 + $firstDerivative * $firstDerivative, 1.5))
|
|
$maximum = [Math]::Max($maximum, $curvature)
|
|
}
|
|
return $maximum
|
|
}
|
|
|
|
function New-QuinticDirectionSegment(
|
|
[double]$C2,
|
|
[double]$C3,
|
|
[double]$C4,
|
|
[double]$C5,
|
|
[double]$DistributionPower,
|
|
[int]$Samples = 400) {
|
|
$points = New-Object System.Collections.Generic.List[object]
|
|
$arcLength = 0.0
|
|
$previousX = 0.0
|
|
$previousY = 0.0
|
|
for ($index = 0; $index -lt $Samples; $index++) {
|
|
$t = [Math]::Pow($index / [double]($Samples - 1), $DistributionPower)
|
|
$x = 1.0 + $t
|
|
$y = $C2 * $t * $t + $C3 * $t * $t * $t + $C4 * $t * $t * $t * $t + $C5 * $t * $t * $t * $t * $t
|
|
if ($index -gt 0) {
|
|
$arcLength += [Math]::Sqrt(($x - $previousX) * ($x - $previousX) + ($y - $previousY) * ($y - $previousY))
|
|
}
|
|
$heading = [Math]::Atan2(
|
|
2.0 * $C2 * $t + 3.0 * $C3 * $t * $t + 4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t,
|
|
1.0)
|
|
[void]$points.Add((New-GeometryPoint $x $y $arcLength $heading $heading))
|
|
$previousX = $x
|
|
$previousY = $y
|
|
}
|
|
return New-DirectionSegment 0 $forward $points.ToArray()
|
|
}
|
|
|
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|
$processing = $root + 'Processing.'
|
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
|
|
$analyzerType = Get-RequiredType ($processing + 'PathGeometryAnalyzer')
|
|
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
|
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
|
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
|
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
|
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
|
$analysisType = Get-RequiredType ($processing + 'PathGeometryAnalysis')
|
|
$preprocessorType = Get-RequiredType ($processing + 'PathSmoothingPreprocessor')
|
|
$resamplerType = Get-RequiredType ($processing + 'ArcLengthResampler')
|
|
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
|
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
|
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
|
$coarseSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
|
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
|
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
|
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
|
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
|
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
|
$smoothingConfigurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
|
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
|
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
|
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
|
$validatorType = Get-RequiredType ($root + 'Validation.SmoothedPathValidator')
|
|
$rawBaselineBuilderType = Get-RequiredType ($processing + 'RawPathBaselineBuilder')
|
|
|
|
Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
|
|
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
|
|
Assert-True ($null -ne $resamplerType) 'ArcLengthResampler must be discoverable for deterministic resampling.'
|
|
Assert-True ($null -ne $analysisType.GetProperty('MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter')) `
|
|
'PathGeometryAnalysis must expose peak d-kappa/d-s.'
|
|
|
|
$analyzer = [Activator]::CreateInstance($analyzerType)
|
|
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
|
|
Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
|
|
Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
|
|
$validator = [Activator]::CreateInstance($validatorType)
|
|
$validateMethod = $validatorType.GetMethod('TryValidate')
|
|
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
|
|
Assert-Equal 9 $validateMethod.GetParameters().Length 'SmoothedPathValidator.TryValidate must retain its public contract.'
|
|
$rawBaselineMethod = $rawBaselineBuilderType.GetMethod(
|
|
'TryCreate',
|
|
[Reflection.BindingFlags]'Static,NonPublic')
|
|
Assert-True ($null -ne $rawBaselineMethod) 'RawPathBaselineBuilder must expose its internal TryCreate path.'
|
|
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
|
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
|
|
|
|
# The chord-corrected estimator must retain an exact circular curvature limit for both travel directions.
|
|
$maximumAllowedCurvature = 5.0 / 6.0
|
|
$circleVehicle = [Activator]::CreateInstance($vehicleType)
|
|
$circleVehicle.LengthMeters = 0.20
|
|
$circleVehicle.WidthMeters = 0.20
|
|
$circleVehicle.SafetyMarginMeters = 0.0
|
|
$circleVehicle.MaximumCurvaturePerMeter = $maximumAllowedCurvature
|
|
foreach ($direction in @($forward, $reverse)) {
|
|
$exactLimitCircle = New-CircularDirectionSegment $direction $maximumAllowedCurvature 20 0.05
|
|
$exactLimitAnalysis = Invoke-Analysis @($exactLimitCircle)
|
|
foreach ($point in $exactLimitAnalysis.Path) {
|
|
Assert-True ([Math]::Abs($point.VehicleCurvature) -le $maximumAllowedCurvature + 1.0e-9) `
|
|
('An exact-limit ' + $direction + ' circle must not exceed the curvature limit.')
|
|
}
|
|
$validation = Invoke-GeometryValidation $exactLimitAnalysis @($exactLimitCircle) $circleVehicle
|
|
Assert-True $validation.Accepted ('The validator must accept an analyzed exact-limit ' + $direction + ' circle. Reason=' + $validation.Reason)
|
|
}
|
|
|
|
# An analyzed over-limit circle must remain detectable by the unchanged validator threshold.
|
|
$overLimitCircle = New-CircularDirectionSegment $forward ($maximumAllowedCurvature + 0.01) 20 0.05
|
|
$overLimitAnalysis = Invoke-Analysis @($overLimitCircle)
|
|
Assert-True ($overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt $maximumAllowedCurvature + 1.0e-6) `
|
|
'An over-limit circle must exceed the vehicle curvature limit by more than the validator tolerance.'
|
|
$overLimitValidation = Invoke-GeometryValidation $overLimitAnalysis @($overLimitCircle) $circleVehicle
|
|
Assert-False $overLimitValidation.Accepted 'The validator must reject an analyzed over-limit circle.'
|
|
|
|
# Trusted raw fallback must apply the exact vehicle-curvature gate before reconstruction.
|
|
$trustedLimit = 0.80
|
|
$trustedOverLimit = $trustedLimit + 0.0000005
|
|
$analyzedCurvature = $trustedLimit + 0.01
|
|
$chordLength = 0.05
|
|
$headingStep = 2.0 * [Math]::Asin($analyzedCurvature * $chordLength / 2.0)
|
|
$radius = 1.0 / $analyzedCurvature
|
|
$trustedPoints = [Array]::CreateInstance($coarsePointType, 21)
|
|
for ($index = 0; $index -le 20; $index++) {
|
|
$theta = $headingStep * $index
|
|
$trustedPoints.SetValue((New-CoarsePathPoint `
|
|
(1.0 + $radius * [Math]::Sin($theta)) `
|
|
(1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
|
|
($index * $chordLength) `
|
|
$forward `
|
|
$false `
|
|
$(if ($index -eq 0) { 'Start' } else { 'MotionPrimitive' }) `
|
|
$theta `
|
|
$trustedOverLimit), $index)
|
|
}
|
|
$trustedSegments = [Array]::CreateInstance($coarseSegmentType, 1)
|
|
$trustedSegments.SetValue(
|
|
[Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 20, $false, $false)),
|
|
0)
|
|
$trustedVehicle = [Activator]::CreateInstance($vehicleType)
|
|
$trustedVehicle.LengthMeters = 0.20
|
|
$trustedVehicle.WidthMeters = 0.20
|
|
$trustedVehicle.SafetyMarginMeters = 0.0
|
|
$trustedVehicle.MaximumCurvaturePerMeter = $trustedLimit
|
|
$trustedConfiguration = [Activator]::CreateInstance($smoothingConfigurationType)
|
|
$trustedRequest = [Activator]::CreateInstance($smoothingRequestType, @(
|
|
$trustedPoints,
|
|
$trustedSegments,
|
|
(New-EmptyGeometryMap),
|
|
$trustedVehicle,
|
|
$trustedConfiguration))
|
|
$trustedPreprocessor = [Activator]::CreateInstance($preprocessorType)
|
|
$trustedPrepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
|
$trustedPrepareArguments = [object[]]@($trustedRequest, $null, $null)
|
|
Assert-True $trustedPrepareMethod.Invoke($trustedPreprocessor, $trustedPrepareArguments) `
|
|
('Trusted raw fallback regression must prepare successfully. Reason=' + $trustedPrepareArguments[2])
|
|
$trustedBaselineArguments = [object[]]@(
|
|
$trustedRequest,
|
|
$trustedPrepareArguments[1],
|
|
$analyzer,
|
|
$trustedConfiguration.OutputSpacingMeters,
|
|
$validator,
|
|
$trustedConfiguration.MaximumCollisionCheckStepMeters,
|
|
$null,
|
|
$null)
|
|
Assert-False $rawBaselineMethod.Invoke($null, $trustedBaselineArguments) `
|
|
'Trusted raw fallback must not publish a coarse curvature above the exact vehicle maximum.'
|
|
Assert-Equal $overLimitValidation.Reason $trustedBaselineArguments[7] `
|
|
'Rejecting trusted raw fallback must preserve the original analyzed-path validation failure.'
|
|
|
|
# The chord-corrected estimator must not under-estimate these smooth references more than the former polyline estimator.
|
|
foreach ($quinticCase in @(
|
|
[pscustomobject]@{ Name = 'SBend'; C2 = 0.0; C3 = 0.30; C4 = -0.45; C5 = 0.18; Power = 1.0 },
|
|
[pscustomobject]@{ Name = 'EndpointPeak'; C2 = 0.18; C3 = -0.12; C4 = 0.0; C5 = 0.0; Power = 1.0 },
|
|
[pscustomobject]@{ Name = 'NonUniformFinalInterval'; C2 = -0.12; C3 = 0.36; C4 = -0.30; C5 = 0.08; Power = 1.7 })) {
|
|
$quintic = New-QuinticDirectionSegment $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5 $quinticCase.Power
|
|
$quinticAnalysis = Invoke-Analysis @($quintic)
|
|
$analyticMaximum = Get-QuinticAnalyticCurvatureMaximum $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5
|
|
$newDeficit = [Math]::Max(0.0, $analyticMaximum - $quinticAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter)
|
|
$oldDeficit = [Math]::Max(0.0, $analyticMaximum - (Get-OldPolylineCurvatureMaximum $quinticAnalysis))
|
|
Assert-True ($newDeficit -le $oldDeficit + 1.0e-6) `
|
|
($quinticCase.Name + ' must not have greater one-sided curvature under-estimation than the old polyline estimator.')
|
|
}
|
|
|
|
# A half-turn over a single chord has no unambiguous geometric curvature estimate.
|
|
$ambiguousTurn = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.0 1.0 ([Math]::PI) ([Math]::PI)))
|
|
Assert-AnalysisRejected @($ambiguousTurn) 'A two-point heading turn of π must be rejected as ambiguous.'
|
|
|
|
# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
|
|
$straight = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
|
$straightAnalysis = Invoke-Analysis @($straight)
|
|
Assert-Equal 21 $straightAnalysis.Path.Count 'A one-metre straight must produce twenty 0.05 m intervals plus the initial point.'
|
|
Assert-Near 0.0 $straightAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 0.000000001 `
|
|
'A straight must have zero peak d-kappa/d-s.'
|
|
foreach ($point in $straightAnalysis.Path) {
|
|
Assert-Near 0.0 $point.VehicleCurvatureDerivative 0.000000001 `
|
|
'A straight point must carry zero d-kappa/d-s.'
|
|
}
|
|
for ($index = 1; $index -lt $straightAnalysis.Path.Count; $index++) {
|
|
$left = $straightAnalysis.Path[$index - 1]
|
|
$right = $straightAnalysis.Path[$index]
|
|
$distance = [Math]::Sqrt(($right.X - $left.X) * ($right.X - $left.X) + ($right.Y - $left.Y) * ($right.Y - $left.Y))
|
|
Assert-Near 0.05 $distance 0.000000001 'Straight resampling intervals must be exactly 0.05 m.'
|
|
Assert-Near 0.0 $right.GeometricCurvature 0.000000001 'A forward straight must have zero geometric curvature.'
|
|
Assert-Near 0.0 $right.VehicleCurvature 0.000000001 'A forward straight must have zero vehicle curvature.'
|
|
}
|
|
$straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
|
|
Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
|
|
Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'
|
|
|
|
# Raw coarse anchors carry the vehicle pose, which may differ slightly from the chord tangent of a finite integration step.
|
|
$poseAnchoredCurve = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.2 1.0 0.4 0.4))
|
|
$poseAnchoredAnalysis = Invoke-Analysis @($poseAnchoredCurve)
|
|
Assert-Near 0.0 $poseAnchoredAnalysis.Path[0].Heading 0.000000000001 'Geometry analysis must retain the first coarse-anchor heading rather than replace it with a chord tangent.'
|
|
$poseAnchoredEnd = $poseAnchoredAnalysis.Path[$poseAnchoredAnalysis.Path.Count - 1]
|
|
Assert-Near 0.4 $poseAnchoredEnd.Heading 0.000000000001 'Geometry analysis must retain the final coarse-anchor heading rather than replace it with a chord tangent.'
|
|
|
|
# A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
|
|
$forwardArcPoints = New-Object System.Collections.Generic.List[object]
|
|
for ($index = 0; $index -le 32; $index++) {
|
|
$theta = ([Math]::PI / 2.0) * $index / 32.0
|
|
$x = 2.0 * [Math]::Sin($theta)
|
|
$y = 2.0 * (1.0 - [Math]::Cos($theta))
|
|
$arcLength = 2.0 * $theta
|
|
[void]$forwardArcPoints.Add((New-GeometryPoint $x $y $arcLength $theta $theta))
|
|
}
|
|
$forwardArc = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray()
|
|
$forwardArcAnalysis = Invoke-Analysis @($forwardArc)
|
|
$forwardArcMidpoint = $forwardArcAnalysis.Path[[int]($forwardArcAnalysis.Path.Count / 2)]
|
|
Assert-Near 0.5 $forwardArcMidpoint.GeometricCurvature 0.01 'An R=2 quarter circle must have geometric curvature +0.5 1/m.'
|
|
Assert-Near 0.5 $forwardArcMidpoint.VehicleCurvature 0.01 'A forward R=2 quarter circle must have vehicle curvature +0.5 1/m.'
|
|
|
|
# The same spatial R=2 circle in reverse retains geometric curvature but negates vehicle curvature.
|
|
$reverseArc = New-DirectionSegment 0 $reverse $forwardArcPoints.ToArray()
|
|
$reverseArcAnalysis = Invoke-Analysis @($reverseArc)
|
|
$reverseArcMidpoint = $reverseArcAnalysis.Path[[int]($reverseArcAnalysis.Path.Count / 2)]
|
|
Assert-Near 0.5 $reverseArcMidpoint.GeometricCurvature 0.01 'Reverse travel must not change geometric curvature.'
|
|
Assert-Near -0.5 $reverseArcMidpoint.VehicleCurvature 0.01 'A reverse R=2 quarter circle must have vehicle curvature -0.5 1/m.'
|
|
|
|
# Gear-switch poses are intentionally duplicated: they keep equal arc length and never enter a derivative denominator.
|
|
$forwardBeforeSwitch = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true)) $false $true
|
|
$reverseAfterSwitch = New-DirectionSegment 1 $reverse @(
|
|
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true),
|
|
(New-GeometryPoint 0.0 0.0 2.0 0.0 0.0)) $true $false
|
|
$switchAnalysis = Invoke-Analysis @($forwardBeforeSwitch, $reverseAfterSwitch)
|
|
$firstSegment = $switchAnalysis.Segments[0]
|
|
$secondSegment = $switchAnalysis.Segments[1]
|
|
$switchLeft = $switchAnalysis.Path[$firstSegment.EndIndex]
|
|
$switchRight = $switchAnalysis.Path[$secondSegment.StartIndex]
|
|
Assert-Near $switchLeft.X $switchRight.X 0.0 'Gear-switch endpoints must retain duplicate X coordinates.'
|
|
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'Gear-switch endpoints must retain duplicate Y coordinates.'
|
|
Assert-Near $switchLeft.ArcLength $switchRight.ArcLength 0.0 'Gear-switch endpoints must retain duplicate arc length.'
|
|
Assert-Equal 'Forward' $switchLeft.Direction.ToString() 'The first gear-switch pose must retain its forward segment direction.'
|
|
Assert-Equal 'Reverse' $switchRight.Direction.ToString() 'The second gear-switch pose must retain its reverse segment direction.'
|
|
Assert-True (-not [double]::IsNaN($switchLeft.GeometricCurvature)) 'No derivative may cross the gear-switch duplicate point.'
|
|
Assert-True (-not [double]::IsNaN($switchRight.GeometricCurvature)) 'No reverse derivative may cross the gear-switch duplicate point.'
|
|
Assert-True (-not [double]::IsNaN($switchLeft.VehicleCurvatureDerivative)) `
|
|
'The forward side of a gear switch must have a finite one-sided derivative.'
|
|
Assert-True (-not [double]::IsNaN($switchRight.VehicleCurvatureDerivative)) `
|
|
'The reverse side of a gear switch must have a finite one-sided derivative.'
|
|
|
|
# Curvature at a curved segment's end and the following straight reverse segment's start must remain independently differentiated.
|
|
$forwardArcToSwitch = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray() $false $true
|
|
$reverseStraightAfterArc = New-DirectionSegment 1 $reverse @(
|
|
(New-GeometryPoint 2.0 2.0 0.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0) $true),
|
|
(New-GeometryPoint 2.0 1.0 1.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0))) $true $false
|
|
$curveSwitchAnalysis = Invoke-Analysis @($forwardArcToSwitch, $reverseStraightAfterArc)
|
|
$reverseStraightStart = $curveSwitchAnalysis.Path[$curveSwitchAnalysis.Segments[1].StartIndex]
|
|
Assert-Near 0.0 $reverseStraightStart.GeometricCurvature 0.000000001 'A gear-switch must not use the preceding curve to differentiate a reverse straight segment.'
|
|
Assert-Near 0.0 $reverseStraightStart.VehicleCurvatureDerivative 0.000000001 `
|
|
'No curvature derivative may cross from the preceding forward arc into a reverse straight.'
|
|
|
|
$physicalStart = New-DirectionSegmentWithStartCurvature 0 $forward 0.20 @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
|
$physicalStartAnalysis = Invoke-Analysis @($physicalStart)
|
|
Assert-Near 0.20 $physicalStartAnalysis.Path[0].VehicleCurvature 0.000000001 `
|
|
'The unified analyzer must retain a real start steering-curvature boundary state.'
|
|
Assert-True ($physicalStartAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter -gt 0.0) `
|
|
'A real start-curvature mismatch must remain visible to the quality analyzer.'
|
|
|
|
# A boundary that claims a gear switch must be a duplicated pose with opposite direction; discontinuities are rejected.
|
|
$invalidSwitch = New-DirectionSegment 1 $reverse @(
|
|
(New-GeometryPoint 1.2 0.0 1.0 0.0 0.0 $true),
|
|
(New-GeometryPoint 0.2 0.0 2.0 0.0 0.0)) $true $false
|
|
Assert-AnalysisRejected @($forwardBeforeSwitch, $invalidSwitch) 'A discontinuous gear-switch boundary must be rejected.'
|
|
$trailingGearSwitch = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0)) $false $true
|
|
Assert-AnalysisRejected @($trailingGearSwitch) 'The final direction segment must not advertise a non-existent trailing gear switch.'
|
|
|
|
# The public preprocessor receives a raw coarse path and resets every prepared direction segment to local arc length zero.
|
|
$coarsePoints = [Array]::CreateInstance($coarsePointType, 4)
|
|
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 0.0 $forward), 0)
|
|
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $forward), 1)
|
|
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $reverse $true), 2)
|
|
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 2.0 $reverse), 3)
|
|
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
|
|
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
|
|
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = 1.0
|
|
$vehicle.WidthMeters = 0.5
|
|
$vehicle.SafetyMarginMeters = 0.0
|
|
$vehicle.MaximumCurvaturePerMeter = 1.0
|
|
$configuration = [Activator]::CreateInstance($configurationType)
|
|
$uninitializedMap = [System.Runtime.Serialization.FormatterServices]::GetUninitializedObject($mapType)
|
|
$request = [Activator]::CreateInstance($requestType, @($coarsePoints, $coarseSegments, $uninitializedMap, $vehicle, $configuration))
|
|
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
|
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
|
$prepareArguments = [object[]]@($request, $null, $null)
|
|
$prepared = $prepareMethod.Invoke($preprocessor, $prepareArguments)
|
|
Assert-True $prepared ('Preprocessor must accept a legal forward/reverse raw coarse path. Reason=' + $prepareArguments[2])
|
|
Assert-Near 0.0 $prepareArguments[1].Segments[1].Points[0].ArcLength 0.0 'Every prepared direction segment must begin at local arc length zero.'
|
|
Assert-True $prepareArguments[1].Segments[1].Points[0].IsGearSwitchPoint 'The reverse prepared segment must retain its gear-switch point.'
|
|
|
|
# Finite coordinates can still overflow distance arithmetic; public resampling must reject them instead of emitting NaN/Infinity.
|
|
$overflowSegment = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint -1.0e308 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0e308 0.0 1.0 0.0 0.0))
|
|
$resampler = [Activator]::CreateInstance($resamplerType)
|
|
$segmentResampleMethod = @($resamplerType.GetMethods() | Where-Object {
|
|
$_.Name -eq 'TryResample' -and $_.GetParameters()[0].ParameterType -eq $segmentType
|
|
})[0]
|
|
$resampleArguments = [object[]]@($overflowSegment, [double]0.05, $null, $null)
|
|
$resampled = $segmentResampleMethod.Invoke($resampler, $resampleArguments)
|
|
Assert-True (-not $resampled) ('Resampling must reject a distance overflow. Reason=' + $resampleArguments[3])
|
|
|
|
# A prepared path must reject null direction segments rather than silently dropping them during flattening.
|
|
$nullSegmentArray = [Array]::CreateInstance($segmentType, 1)
|
|
$nullSegmentRejected = $false
|
|
try { [void][Activator]::CreateInstance($preparedPathType, @($nullSegmentArray)) } catch { $nullSegmentRejected = $true }
|
|
Assert-True $nullSegmentRejected 'PreparedPath must reject a null direction segment.'
|
|
|
|
# Unwrapped heading must not jump by 2π when tangents cross the -π/π branch cut.
|
|
$crossing = New-DirectionSegment 0 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
|
|
(New-GeometryPoint -1.0 ([Math]::Tan(10.0 * [Math]::PI / 180.0)) 1.015 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
|
|
(New-GeometryPoint -2.0 0.0 2.03 (-170.0 * [Math]::PI / 180.0) (-170.0 * [Math]::PI / 180.0)))
|
|
$crossingAnalysis = Invoke-Analysis @($crossing)
|
|
for ($index = 1; $index -lt $crossingAnalysis.Path.Count; $index++) {
|
|
$difference = [Math]::Abs($crossingAnalysis.Path[$index].UnwrappedHeading - $crossingAnalysis.Path[$index - 1].UnwrappedHeading)
|
|
Assert-True ($difference -lt [Math]::PI) 'Unwrapped headings must remain continuous across the ±π branch cut.'
|
|
}
|
|
|
|
# The request preprocessor must reset arc length independently for every direction segment,
|
|
# while retaining the duplicated pose that represents a legal forward-to-reverse gear switch.
|
|
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
|
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
|
Assert-True ($null -ne $prepareMethod) 'PathSmoothingPreprocessor must expose TryPrepare.'
|
|
$coarsePath = [Array]::CreateInstance($coarsePointType, 5)
|
|
$coarsePath.SetValue((New-CoarsePathPoint 0.0 0.0 0.0 $forward $false 'Start'), 0)
|
|
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 1.0 $forward), 1)
|
|
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $forward), 2)
|
|
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $reverse $true), 3)
|
|
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 3.0 $reverse), 4)
|
|
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
|
|
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 2, $false, $true)), 0)
|
|
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 3, 4, $true, $false)), 1)
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = [double]0.80
|
|
$vehicle.WidthMeters = [double]0.60
|
|
$vehicle.SafetyMarginMeters = [double]0.05
|
|
$vehicle.MaximumCurvaturePerMeter = [double]0.80
|
|
$configuration = [Activator]::CreateInstance($smoothingConfigurationType)
|
|
$smoothingRequest = [Activator]::CreateInstance($smoothingRequestType, @(
|
|
$coarsePath, $coarseSegments, (New-EmptyGeometryMap), $vehicle, $configuration))
|
|
$prepareArguments = [object[]]@($smoothingRequest, $null, $null)
|
|
Assert-True $prepareMethod.Invoke($preprocessor, $prepareArguments) ('Preprocessor must accept legal forward/reverse topology. Reason=' + $prepareArguments[2])
|
|
$preparedPath = $prepareArguments[1]
|
|
Assert-Equal 2 $preparedPath.Segments.Count 'Preprocessor must preserve both direction segments.'
|
|
Assert-Near 0.0 $preparedPath.Segments[1].Points[0].ArcLength 0.0 'The reverse segment must restart local arc length at zero.'
|
|
Assert-True $preparedPath.Segments[1].Points[0].IsGearSwitchPoint 'The duplicate reverse gear-switch point must be retained.'
|
|
|
|
# Segments may meet only at a paired, coincident forward/reverse gear switch.
|
|
$illegalGearJump = New-DirectionSegment 1 $reverse @(
|
|
(New-GeometryPoint 1.25 0.0 1.0 0.0 0.0 $true),
|
|
(New-GeometryPoint 0.25 0.0 2.0 0.0 0.0)) $true $false
|
|
Invoke-RejectedAnalysis @($forwardBeforeSwitch, $illegalGearJump) 'A gear-switch boundary whose poses differ must be rejected.'
|
|
$illegalNormalBoundary = New-DirectionSegment 1 $forward @(
|
|
(New-GeometryPoint 2.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 3.0 0.0 1.0 0.0 0.0)) $false $false
|
|
Invoke-RejectedAnalysis @($straight, $illegalNormalBoundary) 'A non-gear segment boundary must be rejected.'
|
|
|
|
# Finite endpoint coordinates can still overflow while computing their separation; reject before interpolation.
|
|
$resampler = [Activator]::CreateInstance($resamplerType)
|
|
$resamplePointsMethod = $resamplerType.GetMethods() | Where-Object {
|
|
$_.Name -eq 'TryResample' -and $_.GetParameters().Length -eq 4 -and
|
|
$_.GetParameters()[0].ParameterType -eq [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($pointType)
|
|
} | Select-Object -First 1
|
|
Assert-True ($null -ne $resamplePointsMethod) 'ArcLengthResampler must expose point-list TryResample.'
|
|
$hugePoints = [Array]::CreateInstance($pointType, 2)
|
|
$hugeCoordinate = [double]::MaxValue / 2.0
|
|
$hugePoints.SetValue((New-GeometryPoint (-$hugeCoordinate) 0.0 0.0 0.0 0.0), 0)
|
|
$hugePoints.SetValue((New-GeometryPoint $hugeCoordinate 0.0 1.0 0.0 0.0), 1)
|
|
$resampleArguments = [object[]]@($hugePoints, [double]0.05, $null, $null)
|
|
Assert-False $resamplePointsMethod.Invoke($resampler, $resampleArguments) 'Resampling must reject an infinite geometric distance caused by finite coordinates.'
|
|
|
|
# PreparedPath is an all-or-nothing immutable topology snapshot: null direction segments are invalid.
|
|
$nullPreparedSegments = [Array]::CreateInstance($segmentType, 1)
|
|
Assert-Throws { [Activator]::CreateInstance($preparedPathType, @($nullPreparedSegments)) } 'PreparedPath must reject null direction segments.'
|
|
|
|
# Segment indices are deliberately dense and equal to their position in the candidate array.
|
|
$sparseSegment = New-DirectionSegment 2 $forward @(
|
|
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
|
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
|
Invoke-RejectedAnalysis @($sparseSegment) 'Prepared direction-segment indices must match their dense array position.'
|
|
|
|
Write-Output 'Path smoothing geometry checks passed.'
|