ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs

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;
    }

    /// <summary>从两个端点的位置、一阶导数和二阶导数创建参数区间 [0, 1] 上的五次 Hermite 曲线。</summary>
    /// <param name="x0">起点 X 坐标，单位 m。</param>
    /// <param name="y0">起点 Y 坐标，单位 m。</param>
    /// <param name="dx0">起点对参数的 X 一阶导数。</param>
    /// <param name="dy0">起点对参数的 Y 一阶导数。</param>
    /// <param name="ddx0">起点对参数的 X 二阶导数。</param>
    /// <param name="ddy0">起点对参数的 Y 二阶导数。</param>
    /// <param name="x1">终点 X 坐标，单位 m。</param>
    /// <param name="y1">终点 Y 坐标，单位 m。</param>
    /// <param name="dx1">终点对参数的 X 一阶导数。</param>
    /// <param name="dy1">终点对参数的 Y 一阶导数。</param>
    /// <param name="ddx1">终点对参数的 X 二阶导数。</param>
    /// <param name="ddy1">终点对参数的 Y 二阶导数。</param>
    /// <param name="curve">成功时为可求值曲线；失败时为 <see langword="null"/>。</param>
    /// <param name="reason">失败原因；成功时为空字符串。</param>
    /// <returns>全部边界条件有限且一阶导数非零时为 <see langword="true"/>；否则为 <see langword="false"/>。</returns>
    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;
    }

    /// <summary>在归一化参数处求曲线位置及其一阶、二阶导数。</summary>
    /// <param name="u">闭区间 [0, 1] 内的无量纲曲线参数。</param>
    /// <param name="x">返回位置 X，单位 m。</param>
    /// <param name="y">返回位置 Y，单位 m。</param>
    /// <param name="dx">返回 X 对参数的一阶导数。</param>
    /// <param name="dy">返回 Y 对参数的一阶导数。</param>
    /// <param name="ddx">返回 X 对参数的二阶导数。</param>
    /// <param name="ddy">返回 Y 对参数的二阶导数。</param>
    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);
}