cubic_hermite_spline.h

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

class CubicHermiteSpline : public HermiteSpline<3> {
 public:
  CubicHermiteSpline() = default;

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

  CubicHermiteSpline(const Translation2d& start_pos, const Translation2d& end_pos,
                     const Translation2d& start_tangent, const Translation2d& end_tangent,
                     double max_err = 1e-4) {
    this->x_ = cubic_coeffs(start_pos.x(units::in), end_pos.x(units::in),
                            start_tangent.x(units::in), end_tangent.x(units::in));
    this->y_ = cubic_coeffs(start_pos.y(units::in), end_pos.y(units::in),
                            start_tangent.y(units::in), end_tangent.y(units::in));
    build_arc_table(max_err);
  }
 
 private:
  static std::array<double, 4> cubic_coeffs(double start_pos, double end_pos, double start_tangent,
                                            double end_tangent) {
    return {
        start_pos,
        start_tangent,
        -3.0 * start_pos + 3.0 * end_pos - 2.0 * start_tangent - end_tangent,
        2.0 * start_pos - 2.0 * end_pos + start_tangent + end_tangent,
    };
  }
};