From a3b6ea7d844974a28c4eaa91c50f92520caefd19 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 30 Jul 2026 17:07:22 +0800 Subject: [PATCH] feat: add quintic Hermite curve geometry --- .../LocalG2/QuinticHermiteCurve2D.cs | 124 ++++++++++++++++++ .../verify_path_smoothing_local_g2_curve.ps1 | 118 +++++++++++++++++ 2 files changed, 242 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs create mode 100644 ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs new file mode 100644 index 0000000..006189f --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs @@ -0,0 +1,124 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; + +/// 满足两个二维端点二阶边界条件的参数五次 Hermite 曲线。 +internal sealed class QuinticHermiteCurve2D +{ + private readonly double _x0; + private readonly double _x1; + private readonly double _x2; + private readonly double _x3; + private readonly double _x4; + private readonly double _x5; + private readonly double _y0; + private readonly double _y1; + private readonly double _y2; + private readonly double _y3; + private readonly double _y4; + private readonly double _y5; + + private QuinticHermiteCurve2D( + double x0, double x1, double x2, double x3, double x4, double x5, + double y0, double y1, double y2, double y3, double y4, double y5) + { + _x0 = x0; + _x1 = x1; + _x2 = x2; + _x3 = x3; + _x4 = x4; + _x5 = x5; + _y0 = y0; + _y1 = y1; + _y2 = y2; + _y3 = y3; + _y4 = y4; + _y5 = y5; + } + + internal static bool TryCreate( + double x0, double y0, double dx0, double dy0, double ddx0, double ddy0, + double x1, double y1, double dx1, double dy1, double ddx1, double ddy1, + out QuinticHermiteCurve2D curve, + out string reason) + { + curve = null; + reason = string.Empty; + if (!AreFinite(x0, y0, dx0, dy0, ddx0, ddy0, x1, y1, dx1, dy1, ddx1, ddy1)) + { + reason = "五次 Hermite 曲线需要有限的边界条件。"; + return false; + } + if (IsZeroVector(dx0, dy0) || IsZeroVector(dx1, dy1)) + { + reason = "五次 Hermite 曲线端点一阶导数不能为零。"; + return false; + } + + SolveCoordinate(x0, dx0, ddx0, x1, dx1, ddx1, + out double ax0, out double ax1, out double ax2, out double ax3, out double ax4, out double ax5); + SolveCoordinate(y0, dy0, ddy0, y1, dy1, ddy1, + out double ay0, out double ay1, out double ay2, out double ay3, out double ay4, out double ay5); + if (!AreFinite(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5)) + { + reason = "五次 Hermite 曲线系数溢出。"; + return false; + } + + curve = new QuinticHermiteCurve2D(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5); + return true; + } + + internal void Evaluate( + double u, + out double x, out double y, + out double dx, out double dy, + out double ddx, out double ddy) + { + if (!IsFinite(u) || u < 0d || u > 1d) + throw new ArgumentOutOfRangeException(nameof(u), "The curve parameter must be finite and within [0, 1]."); + + x = EvaluateValue(_x0, _x1, _x2, _x3, _x4, _x5, u); + y = EvaluateValue(_y0, _y1, _y2, _y3, _y4, _y5, u); + dx = EvaluateFirstDerivative(_x1, _x2, _x3, _x4, _x5, u); + dy = EvaluateFirstDerivative(_y1, _y2, _y3, _y4, _y5, u); + ddx = EvaluateSecondDerivative(_x2, _x3, _x4, _x5, u); + ddy = EvaluateSecondDerivative(_y2, _y3, _y4, _y5, u); + } + + private static void SolveCoordinate(double p0, double v0, double acceleration0, double p1, double v1, double acceleration1, + out double a0, out double a1, out double a2, out double a3, out double a4, out double a5) + { + a0 = p0; + a1 = v0; + a2 = acceleration0 / 2d; + double c0 = p1 - (a0 + a1 + a2); + double c1 = v1 - (a1 + 2d * a2); + double c2 = acceleration1 - 2d * a2; + a3 = 10d * c0 - 4d * c1 + 0.5d * c2; + a4 = -15d * c0 + 7d * c1 - c2; + a5 = 6d * c0 - 3d * c1 + 0.5d * c2; + } + + private static double EvaluateValue(double a0, double a1, double a2, double a3, double a4, double a5, double u) => + (((((a5 * u + a4) * u + a3) * u + a2) * u + a1) * u) + a0; + + private static double EvaluateFirstDerivative(double a1, double a2, double a3, double a4, double a5, double u) => + ((((5d * a5 * u + 4d * a4) * u + 3d * a3) * u + 2d * a2) * u) + a1; + + private static double EvaluateSecondDerivative(double a2, double a3, double a4, double a5, double u) => + (((20d * a5 * u + 12d * a4) * u + 6d * a3) * u) + 2d * a2; + + private static bool IsZeroVector(double x, double y) => x == 0d && y == 0d; + + private static bool AreFinite(params double[] values) + { + for (int index = 0; index < values.Length; index++) + { + if (!IsFinite(values[index])) return false; + } + return true; + } + + private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value); +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 new file mode 100644 index 0000000..2187dc4 --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 @@ -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.'