fsb_quintic.h

 
#ifndef FSB_QUINTIC_H
#define FSB_QUINTIC_H
 
#include "fsb_types.h"
#include "fsb_trajectory_types.h"
#include "fsb_trajectory_segment.h"
 
namespace fsb
{
 
constexpr real_t QUINTIC_MIN_DURATION = 1e-6;
 
struct QuinticCoeffs
{
    real_t c0 = 0.0; 
    real_t c1 = 0.0; 
    real_t c2 = 0.0; 
    real_t c3 = 0.0; 
    real_t c4 = 0.0; 
    real_t c5 = 0.0; 
};
 
class QuinticTrajectory final : public Segment
{
public:
    QuinticTrajectory() = default;
 
    bool generate(
        real_t start_time, real_t duration, const TrajState& initial_state,
        const TrajState& final_state);
 
    [[nodiscard]] TrajState evaluate(real_t t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_duration);
    }
 
    [[nodiscard]] real_t get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] real_t get_duration() const override
    {
        return m_duration;
    }
 
    [[nodiscard]] real_t get_final_time() const override
    {
        return m_start_time + m_duration;
    }
 
private:
    real_t m_start_time = 0.0;
    real_t m_duration = 0.0;
 
    QuinticCoeffs m_coeffs = {};
};
 
} // namespace fsb
 
#endif