387 lines
22 KiB
PowerShell
387 lines
22 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-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') {
|
|
return [Activator]::CreateInstance($coarsePointType, @(
|
|
$X, $Y, [double]0.0, [double]0.0, $ArcLength,
|
|
$Direction, [double]0.0, [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
|
|
}
|
|
|
|
$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'
|
|
|
|
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.'
|
|
|
|
$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.'
|
|
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
|
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
|
|
|
|
# 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.'
|
|
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.'
|
|
|
|
# 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.'
|
|
|
|
# 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.'
|