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,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);
+}