feat: add quintic Hermite curve geometry

2026-07-30 17:07:22 +08:00
parent 1e307bc89f
commit a3b6ea7d84
2 changed files with 242 additions and 0 deletions
@@ -0,0 +1,118 @@
+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-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-Rejected($Result, [string]$Message) {
+    if ($Result.Accepted) { throw $Message }
+    Assert-True (-not [string]::IsNullOrWhiteSpace($Result.Reason)) 'Rejected curves must provide a stable reason.'
+}
+
+function Assert-Throws([scriptblock]$Action, [string]$Message) {
+    try {
+        & $Action
+    }
+    catch [ArgumentOutOfRangeException] {
+        return
+    }
+    catch [System.Reflection.TargetInvocationException] {
+        Assert-True ($_.Exception.InnerException -is [ArgumentOutOfRangeException]) $Message
+        return
+    }
+    throw $Message
+}
+
+$curveType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.QuinticHermiteCurve2D', $true)
+$tryCreate = $curveType.GetMethod('TryCreate', [Reflection.BindingFlags]'Static,NonPublic')
+$evaluate = $curveType.GetMethod('Evaluate', [Reflection.BindingFlags]'Instance,NonPublic')
+Assert-True ($null -ne $tryCreate) 'QuinticHermiteCurve2D must expose its internal TryCreate factory.'
+Assert-True ($null -ne $evaluate) 'QuinticHermiteCurve2D must expose its internal Evaluate method.'
+
+function Invoke-Curve(
+    [double]$X0, [double]$Y0, [double]$Dx0, [double]$Dy0, [double]$Ddx0, [double]$Ddy0,
+    [double]$X1, [double]$Y1, [double]$Dx1, [double]$Dy1, [double]$Ddx1, [double]$Ddy1) {
+    $arguments = [object[]]@($X0, $Y0, $Dx0, $Dy0, $Ddx0, $Ddy0, $X1, $Y1, $Dx1, $Dy1, $Ddx1, $Ddy1, $null, $null)
+    $accepted = $tryCreate.Invoke($null, $arguments)
+    Assert-True $accepted "Quintic Hermite curve creation must succeed. Reason=$($arguments[13])"
+    return $arguments[12]
+}
+
+function Invoke-InvalidCurve([string]$CaseName) {
+    $arguments = switch ($CaseName) {
+        'ZeroFirstDerivative' { [object[]]@(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
+        'NonFiniteInput' { [object[]]@(0.0, 0.0, [double]::NaN, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
+        default { throw "Unknown invalid curve case: $CaseName" }
+    }
+    return [PSCustomObject]@{
+        Accepted = $tryCreate.Invoke($null, $arguments)
+        Reason = [string]$arguments[13]
+    }
+}
+
+function Invoke-Evaluate($Curve, [double]$U) {
+    $arguments = [object[]]@($U, $null, $null, $null, $null, $null, $null)
+    $evaluate.Invoke($Curve, $arguments)
+    return [PSCustomObject]@{
+        X = [double]$arguments[1]; Y = [double]$arguments[2]
+        Dx = [double]$arguments[3]; Dy = [double]$arguments[4]
+        Ddx = [double]$arguments[5]; Ddy = [double]$arguments[6]
+    }
+}
+
+function Get-Curvature($Sample) {
+    return ($Sample.Dx * $Sample.Ddy - $Sample.Dy * $Sample.Ddx) /
+        [Math]::Pow($Sample.Dx * $Sample.Dx + $Sample.Dy * $Sample.Dy, 1.5)
+}
+
+function Assert-Boundary($Expected, $Actual, [string]$Name) {
+    Assert-Near $Expected.X $Actual.X 1e-10 "$Name X must match."
+    Assert-Near $Expected.Y $Actual.Y 1e-10 "$Name Y must match."
+    Assert-Near $Expected.Dx $Actual.Dx 1e-10 "$Name dX must match."
+    Assert-Near $Expected.Dy $Actual.Dy 1e-10 "$Name dY must match."
+    Assert-Near $Expected.Ddx $Actual.Ddx 1e-10 "$Name ddX must match."
+    Assert-Near $Expected.Ddy $Actual.Ddy 1e-10 "$Name ddY must match."
+}
+
+$straight = Invoke-Curve 0 0 1 0 0 0  2 0 1 0 0 0
+$straightStart = Invoke-Evaluate $straight 0.0
+$straightEnd = Invoke-Evaluate $straight 1.0
+$straightMid = Invoke-Evaluate $straight 0.5
+Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightStart 'Straight start'
+Assert-Boundary ([PSCustomObject]@{ X = 2.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightEnd 'Straight end'
+Assert-Near 0.0 $straightMid.Y 1e-12 'A straight Hermite curve must remain on the axis.'
+
+$curved = Invoke-Curve 0 0 1 0 0 0.4  1 1 0 1 -0.4 0
+$curvedStart = Invoke-Evaluate $curved 0.0
+$curvedEnd = Invoke-Evaluate $curved 1.0
+Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.4 }) $curvedStart 'Curved start'
+Assert-Boundary ([PSCustomObject]@{ X = 1.0; Y = 1.0; Dx = 0.0; Dy = 1.0; Ddx = -0.4; Ddy = 0.0 }) $curvedEnd 'Curved end'
+Assert-Near 0.4 (Get-Curvature $curvedStart) 1e-10 'Start curvature must match boundary derivatives.'
+Assert-Near 0.4 (Get-Curvature $curvedEnd) 1e-10 'End curvature must match boundary derivatives.'
+
+$allFinite = $true
+foreach ($u in @(0.0, 0.25, 0.5, 0.75, 1.0)) {
+    $sample = Invoke-Evaluate $curved $u
+    foreach ($value in @($sample.X, $sample.Y, $sample.Dx, $sample.Dy, $sample.Ddx, $sample.Ddy)) {
+        $allFinite = $allFinite -and (-not [double]::IsNaN($value)) -and (-not [double]::IsInfinity($value))
+    }
+}
+Assert-True $allFinite 'All sampled values must be finite.'
+
+Assert-Rejected (Invoke-InvalidCurve 'ZeroFirstDerivative') `
+    'A curve endpoint with zero first derivative must be rejected.'
+Assert-Rejected (Invoke-InvalidCurve 'NonFiniteInput') `
+    'A curve with a non-finite boundary value must be rejected.'
+Assert-Throws { Invoke-Evaluate $curved -0.001 } 'Evaluate must reject parameters below zero.'
+Assert-Throws { Invoke-Evaluate $curved 1.001 } 'Evaluate must reject parameters above one.'
+
+Write-Output 'Local G2 quintic curve checks passed.'