320 lines
18 KiB
PowerShell
320 lines
18 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 Get-RequiredType([string]$Name) {
|
|
return $assembly.GetType($Name, $true)
|
|
}
|
|
|
|
function Get-PropertyValue($Instance, [string]$Name) {
|
|
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
|
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
|
return $property.GetValue($Instance)
|
|
}
|
|
|
|
function Assert-PointBitwiseEqual($Expected, $Actual, [string]$Message) {
|
|
foreach ($name in @('X', 'Y', 'ArcLength', 'Heading', 'UnwrappedHeading', 'BodyClearance')) {
|
|
$expectedBits = [BitConverter]::DoubleToInt64Bits([double]$Expected.$name)
|
|
$actualBits = [BitConverter]::DoubleToInt64Bits([double]$Actual.$name)
|
|
Assert-Equal $expectedBits $actualBits ($Message + ' ' + $name)
|
|
}
|
|
Assert-Equal $Expected.IsGearSwitchPoint $Actual.IsGearSwitchPoint ($Message + ' IsGearSwitchPoint')
|
|
Assert-Equal $Expected.Source.ToString() $Actual.Source.ToString() ($Message + ' Source')
|
|
}
|
|
|
|
function New-Point(
|
|
[double]$X,
|
|
[double]$Y,
|
|
[double]$ArcLength,
|
|
[double]$Heading,
|
|
[double]$BodyClearance = 0.10,
|
|
[bool]$IsGearSwitch = $false) {
|
|
return [Activator]::CreateInstance($pointType, @(
|
|
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $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-EmptyMap {
|
|
$request = [Activator]::CreateInstance($mapRequestType)
|
|
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
|
$request.ResolutionMm = [single]50
|
|
$request.AllowExplicitEmptyMap = $true
|
|
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
|
Assert-True ($null -ne $map) 'Bézier test must create an explicit empty planning map.'
|
|
return $map
|
|
}
|
|
|
|
function New-AlgorithmInput(
|
|
[object[]]$Segments,
|
|
[double]$ReserveMeters = 0.02,
|
|
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
|
[double]$MaximumWindowLengthMeters = 0.60,
|
|
[double]$HandleLengthRatio = (1.0 / 3.0)) {
|
|
$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))
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = [double]0.20
|
|
$vehicle.WidthMeters = [double]0.20
|
|
$vehicle.SafetyMarginMeters = [double]0.0
|
|
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
|
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
|
$configuration = [Activator]::CreateInstance($configurationType)
|
|
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = $CornerThresholdRadians
|
|
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = $MaximumWindowLengthMeters
|
|
$configuration.LocalCubicBezier.HandleLengthRatio = $HandleLengthRatio
|
|
$options = $optionsConstructor.Invoke(@($configuration))
|
|
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
|
}
|
|
|
|
function Invoke-Candidate(
|
|
[object[]]$Segments,
|
|
[double]$ReserveMeters = 0.02,
|
|
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
|
[double]$MaximumWindowLengthMeters = 0.60,
|
|
[double]$HandleLengthRatio = (1.0 / 3.0),
|
|
[double]$Strength = 1.0) {
|
|
return $smoothMethod.Invoke($smoother, @(
|
|
(New-AlgorithmInput $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio),
|
|
$Strength,
|
|
[Threading.CancellationToken]::None))
|
|
}
|
|
|
|
function Invoke-Smoothing(
|
|
[object[]]$Segments,
|
|
[double]$ReserveMeters = 0.02,
|
|
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
|
[double]$MaximumWindowLengthMeters = 0.60,
|
|
[double]$HandleLengthRatio = (1.0 / 3.0),
|
|
[double]$Strength = 1.0) {
|
|
$candidate = Invoke-Candidate $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio $Strength
|
|
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Bézier smoothing must produce a candidate for the deterministic fixture.'
|
|
return @(Get-PropertyValue $candidate 'Segments')
|
|
}
|
|
|
|
function Get-InterpolatedRunCount($Points) {
|
|
$runCount = 0
|
|
$inRun = $false
|
|
foreach ($point in $Points) {
|
|
$interpolated = $point.Source.ToString() -eq 'Interpolated'
|
|
if ($interpolated -and -not $inRun) { $runCount++ }
|
|
$inRun = $interpolated
|
|
}
|
|
return $runCount
|
|
}
|
|
|
|
function Get-PointAtArcLength($Points, [double]$ArcLength) {
|
|
foreach ($point in $Points) {
|
|
if ([BitConverter]::DoubleToInt64Bits([double]$point.ArcLength) -eq
|
|
[BitConverter]::DoubleToInt64Bits($ArcLength)) {
|
|
return $point
|
|
}
|
|
}
|
|
throw "No output point found at local arc length $ArcLength."
|
|
}
|
|
|
|
function Get-FirstInterpolatedPoint($Points) {
|
|
foreach ($point in $Points) {
|
|
if ($point.Source.ToString() -eq 'Interpolated') { return $point }
|
|
}
|
|
throw 'Expected an interpolated Bézier point.'
|
|
}
|
|
|
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|
$processing = $root + 'Processing.'
|
|
$algorithms = $root + 'Algorithms.'
|
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
|
|
$smootherType = Get-RequiredType ($algorithms + 'LocalCubicBezierSmoother')
|
|
$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')
|
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
|
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
|
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
|
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
|
|
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
|
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
|
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
|
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
|
|
|
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
|
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
|
Assert-True ($null -ne $inputConstructor) 'Bézier tests must construct algorithm input with immutable option values.'
|
|
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
|
Assert-True ($null -ne $optionsConstructor) 'Bézier tests must create immutable option snapshots.'
|
|
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
|
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
|
Assert-True ($null -ne $smoothMethod) 'LocalCubicBezierSmoother must implement the internal smoother contract.'
|
|
Assert-Equal 'LocalCubicBezier' $smoother.Method.ToString() 'Bézier smoother must identify its public smoothing method.'
|
|
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
|
|
|
# A straight path must not create a local Bézier window or alter any sample.
|
|
$straightSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0),
|
|
(New-Point 0.1 0.0 0.1 0.0),
|
|
(New-Point 0.2 0.0 0.2 0.0),
|
|
(New-Point 0.3 0.0 0.3 0.0))
|
|
$straightOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)))[0].Points
|
|
Assert-Equal 0 (Get-InterpolatedRunCount $straightOutput) 'A straight line must create no Bézier replacement window.'
|
|
Assert-Equal $straightSource.Count $straightOutput.Count 'A straight line must retain its original sample count.'
|
|
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
|
Assert-PointBitwiseEqual $straightSource[$index] $straightOutput[$index] 'Straight samples must remain bitwise unchanged.'
|
|
}
|
|
|
|
# One corner is one local replacement: only the corner sample is evaluated while the window endpoints stay fixed.
|
|
$cornerSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0),
|
|
(New-Point 0.1 0.0 0.1 0.0),
|
|
(New-Point 0.2 0.0 0.2 0.0),
|
|
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
|
(New-Point 0.2 0.2 0.4 ([Math]::PI / 2.0)))
|
|
$cornerOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)))[0].Points
|
|
Assert-Equal 1 (Get-InterpolatedRunCount $cornerOutput) 'One corner must produce exactly one contiguous Bézier replacement.'
|
|
Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must preserve the segment sampling topology.'
|
|
Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.'
|
|
Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.'
|
|
Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.'
|
|
$cornerChordLength = [Math]::Sqrt(
|
|
[Math]::Pow($cornerSource[3].X - $cornerSource[1].X, 2.0) +
|
|
[Math]::Pow($cornerSource[3].Y - $cornerSource[1].Y, 2.0))
|
|
$cornerHandleLength = $cornerChordLength / 3.0
|
|
$expectedCornerX =
|
|
0.125 * $cornerSource[1].X +
|
|
0.375 * ($cornerSource[1].X + $cornerHandleLength) +
|
|
0.375 * $cornerSource[3].X +
|
|
0.125 * $cornerSource[3].X
|
|
$expectedCornerY =
|
|
0.125 * $cornerSource[1].Y +
|
|
0.375 * $cornerSource[1].Y +
|
|
0.375 * ($cornerSource[3].Y - $cornerHandleLength) +
|
|
0.125 * $cornerSource[3].Y
|
|
$cornerInterpolated = Get-FirstInterpolatedPoint $cornerOutput
|
|
Assert-Near $expectedCornerX $cornerInterpolated.X 0.000000000001 'Bézier control handles must use the local endpoint chord length for X geometry.'
|
|
Assert-Near $expectedCornerY $cornerInterpolated.Y 0.000000000001 'Bézier control handles must use the local endpoint chord length for Y geometry.'
|
|
|
|
# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits.
|
|
$overlappingSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0),
|
|
(New-Point 0.1 0.0 0.1 0.0),
|
|
(New-Point 0.2 0.0 0.2 0.0),
|
|
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
|
(New-Point 0.3 0.1 0.4 0.0),
|
|
(New-Point 0.4 0.1 0.5 0.0))
|
|
$overlappingOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)))[0].Points
|
|
Assert-Equal 1 (Get-InterpolatedRunCount $overlappingOutput) 'Touching local corner windows must merge into exactly one Bézier replacement.'
|
|
Assert-PointBitwiseEqual $overlappingSource[1] $overlappingOutput[1] 'Merged Bézier entry anchor must remain fixed.'
|
|
Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bézier exit anchor must remain fixed.'
|
|
Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.'
|
|
Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.'
|
|
|
|
# Safe bounded policy: decline an entire connected set when its merged interval exceeds the cap.
|
|
# The two candidate windows below are each 0.20 m, but their merged 0.30 m interval must not
|
|
# produce one over-length curve or be split into new unrequested joins.
|
|
$overCapMergedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)) 0.02 ([Math]::PI / 18.0) 0.20)[0].Points
|
|
Assert-Equal 0 (Get-InterpolatedRunCount $overCapMergedOutput) 'An oversized connected Bézier window set must be declined instead of emitting an over-cap replacement.'
|
|
for ($index = 0; $index -lt $overlappingSource.Count; $index++) {
|
|
Assert-PointBitwiseEqual $overlappingSource[$index] $overCapMergedOutput[$index] 'Declining an oversized connected set must preserve its anchors.'
|
|
}
|
|
|
|
# Samples outside a local window must remain bitwise unchanged rather than be globally re-fit.
|
|
$isolatedSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0),
|
|
(New-Point 0.1 0.0 0.1 0.0),
|
|
(New-Point 0.2 0.0 0.2 0.0),
|
|
(New-Point 0.3 0.0 0.3 0.0),
|
|
(New-Point 0.3 0.1 0.4 ([Math]::PI / 2.0)),
|
|
(New-Point 0.3 0.2 0.5 ([Math]::PI / 2.0)),
|
|
(New-Point 0.3 0.3 0.6 ([Math]::PI / 2.0)))
|
|
$isolatedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $isolatedSource)))[0].Points
|
|
foreach ($index in @(0, 1, 2, 5, 6)) {
|
|
Assert-PointBitwiseEqual $isolatedSource[$index] (Get-PointAtArcLength $isolatedOutput $isolatedSource[$index].ArcLength) 'Samples outside a Bézier window must remain bitwise unchanged.'
|
|
}
|
|
|
|
# Direction segments stay independent; segment endpoints and the gear-switch anchor are fixed.
|
|
$reverseSource = @(
|
|
(New-Point 0.2 0.2 0.0 ([Math]::PI / 2.0) 0.10 $true),
|
|
(New-Point 0.2 0.1 0.1 ([Math]::PI / 2.0)),
|
|
(New-Point 0.2 0.0 0.2 ([Math]::PI / 2.0)))
|
|
$switchOutput = @(Invoke-Smoothing @(
|
|
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
|
(New-DirectionSegment 1 $reverse $reverseSource $true $false)))
|
|
Assert-Equal 2 $switchOutput.Count 'Bézier smoothing must retain separate forward and reverse direction segments.'
|
|
Assert-True $switchOutput[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
|
Assert-True $switchOutput[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
|
Assert-PointBitwiseEqual $cornerSource[0] $switchOutput[0].Points[0] 'Segment start endpoint must remain fixed.'
|
|
Assert-PointBitwiseEqual $cornerSource[$cornerSource.Count - 1] $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] 'Segment end endpoint must remain fixed.'
|
|
Assert-PointBitwiseEqual $reverseSource[0] $switchOutput[1].Points[0] 'Gear-switch point must remain fixed.'
|
|
|
|
# The immutable options each change only their own local behavior.
|
|
$thresholdOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.70)[0].Points
|
|
Assert-Equal 0 (Get-InterpolatedRunCount $thresholdOutput) 'A non-default heading threshold above the corner angle must suppress only corner detection.'
|
|
$windowOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.15)[0].Points
|
|
Assert-Equal 0 (Get-InterpolatedRunCount $windowOutput) 'A non-default maximum window shorter than the local connection must suppress only that window.'
|
|
$shortHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.10)[0].Points
|
|
$longHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.60)[0].Points
|
|
Assert-Equal 1 (Get-InterpolatedRunCount $shortHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
|
Assert-Equal 1 (Get-InterpolatedRunCount $longHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
|
$shortHandlePoint = Get-FirstInterpolatedPoint $shortHandleOutput
|
|
$longHandlePoint = Get-FirstInterpolatedPoint $longHandleOutput
|
|
Assert-True (($shortHandlePoint.X -ne $longHandlePoint.X) -or ($shortHandlePoint.Y -ne $longHandlePoint.Y)) 'A non-default handle ratio must change only the local Bézier geometry.'
|
|
|
|
# Parameter-matched local arc-length reference comparison must reject excess displacement as retryable and publish no geometry.
|
|
$infeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.095
|
|
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToString() 'Exceeded local arc-length displacement must be retryable, not terminal.'
|
|
Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.'
|
|
Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.'
|
|
|
|
# This nonuniform, offset window evaluates its only interior point at t=0.25 and local s=6.
|
|
# A wrong global/index mapping would instead compare to s=5 and accept the 0.50 m clearance;
|
|
# the required local-arc reference at s=6 must reject the roughly 0.65 m displacement.
|
|
$nonuniformOffsetSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0 0.50),
|
|
(New-Point 1.0 0.0 4.0 0.0 0.50),
|
|
(New-Point 2.0 0.0 5.0 0.0 0.50),
|
|
(New-Point 3.0 0.0 6.0 0.0 0.50),
|
|
(New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50),
|
|
(New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50))
|
|
$nonuniformOffsetInfeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformOffsetSource)) 0.0 ([Math]::PI / 18.0) 5.0
|
|
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformOffsetInfeasible 'Status').ToString() 'A nonuniform offset window must use local arc-length mapping for retryable clearance rejection.'
|
|
Assert-True (-not (Get-PropertyValue $nonuniformOffsetInfeasible 'Succeeded')) 'The nonuniform local-arc infeasibility must not be executable.'
|
|
Assert-Equal 0 (Get-PropertyValue $nonuniformOffsetInfeasible 'Segments').Count 'The nonuniform local-arc infeasibility must publish no geometry.'
|
|
|
|
Write-Output 'Path smoothing local cubic Bézier checks passed.'
|