feat: add quintic Hermite curve geometry

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs

@@ -0,0 +1,124 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// <summary>满足两个二维端点二阶边界条件的参数五次 Hermite 曲线。</summary>
+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);
+}

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.'