fsb_cubic.h

 
#ifndef FSB_CUBIC_H
#define FSB_CUBIC_H
 
#include "fsb_types.h"
#include "fsb_trajectory_types.h"
 
namespace fsb
{
 
constexpr Real kCubicMinDuration = 1e-6;
 
struct CubicCoeffs
{
    Real c0 = 0.0; 
    Real c1 = 0.0; 
    Real c2 = 0.0; 
    Real c3 = 0.0; 
};
 
class CubicTrajectory final : public Segment
{
public:
    CubicTrajectory() = default;
 
    bool generate(
        Real start_time, Real duration, const TrajState& initial_state,
        const TrajState& final_state);
 
    void set_coeffs(Real start_time, Real duration, const CubicCoeffs& coeffs);
 
    [[nodiscard]] TrajState evaluate(Real t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_duration);
    }
 
    [[nodiscard]] Real get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] Real get_duration() const override
    {
        return m_duration;
    }
 
    [[nodiscard]] Real get_final_time() const override
    {
        return m_start_time + m_duration;
    }
 
private:
    Real m_start_time = 0.0;
    Real m_duration = 0.0;
 
    CubicCoeffs m_coeffs = {};
};
 
} // namespace fsb
 
#endif