cubic_hermite_spline1d.hpp

// Copyright (c) TrajoptLib contributors
 
#pragma once
 
#include "trajopt/util/symbol_exports.hpp"
 
namespace trajopt {

class TRAJOPT_DLLEXPORT CubicHermiteSpline1d {
 public:
  constexpr CubicHermiteSpline1d(double p0, double p1, double v0, double v1)
      : a{v0 + v1 + 2 * p0 - 2 * p1},
        b{-2 * v0 - v1 - 3 * p0 + 3 * p1},
        c{v0},
        d{p0} {}

  constexpr double get_position(double t) const {
    double t2 = t * t;
    double t3 = t2 * t;
    return a * t3 + b * t2 + c * t + d;
  }

  constexpr double get_velocity(double t) const {
    return 3 * a * t * t + 2 * b * t + c;
  }

  constexpr double get_acceleration(double t) const {
    return 6 * a * t + 2 * b;
  }

  constexpr double get_jerk([[maybe_unused]] double t) const { return 6 * a; }
 
 private:
  // Coefficients of the cubic polynomial
  double a;
  double b;
  double c;
  double d;
};
 
}  // namespace trajopt