quintic_hermite_spline.h

#pragma once
 
#include <array>
 
#include "core/math/spline/cubic_hermite_spline.h"

class QuinticHermiteSpline : public HermiteSpline<5> {
 public:
  QuinticHermiteSpline() = default;

  QuinticHermiteSpline(const HermitePoint& start, const HermitePoint& end, double max_err = 1e-4)
      : QuinticHermiteSpline(start.point, end.point, start.tangent, end.tangent,
                             start.second_derivative, end.second_derivative, max_err) {}

  QuinticHermiteSpline(const Translation2d& start_pos, const Translation2d& end_pos,
                       const Translation2d& start_tangent, const Translation2d& end_tangent,
                       const Translation2d& start_accel, const Translation2d& end_accel,
                       double max_err = 1e-4) {
    this->x_ =
        quintic_coeffs(start_pos.x(units::in), end_pos.x(units::in), start_tangent.x(units::in),
                       end_tangent.x(units::in), start_accel.x(units::in), end_accel.x(units::in));
    this->y_ =
        quintic_coeffs(start_pos.y(units::in), end_pos.y(units::in), start_tangent.y(units::in),
                       end_tangent.y(units::in), start_accel.y(units::in), end_accel.y(units::in));
    build_arc_table(max_err);
  }
 
 private:
  static std::array<double, 6> quintic_coeffs(double start_pos, double end_pos,
                                              double start_tangent, double end_tangent,
                                              double start_accel, double end_accel) {
    return {
        start_pos,
        start_tangent,
        start_accel / 2.0,
        (-20.0 * start_pos + 20.0 * end_pos - 12.0 * start_tangent - 8.0 * end_tangent -
         3.0 * start_accel + end_accel) /
            2.0,
        (30.0 * start_pos - 30.0 * end_pos + 16.0 * start_tangent + 14.0 * end_tangent +
         3.0 * start_accel - 2.0 * end_accel) /
            2.0,
        (-12.0 * start_pos + 12.0 * end_pos - 6.0 * start_tangent - 6.0 * end_tangent -
         start_accel + end_accel) /
            2.0,
    };
  }
};