cmvr-head-client/cpp/common/curve/include/s_curve.h

//
// Created by lgv on 2026/3/2.
//
/**
 * @file s_curve.h
 * @brief 严格点到点位置 S 曲线轨迹生成器
 *
 * 实现标准 7 段式位置 S 曲线速度剖面：
 * 1. 加加速度上升段（加速度逐渐增大）
 * 2. 匀加速段
 * 3. 加加速度下降段（加速度逐渐降到 0）
 * 4. 匀速巡航段
 * 5. 加加速度下降段（减速度逐渐增大）
 * 6. 匀减速段
 * 7. 加加速度上升段（减速度逐渐降到 0）
 *
 * 本类只负责严格点到点 profile 规划与采样，不负责实时目标跟踪。
 */

#pragma once

#include <algorithm>
#include <cmath>
#include <vector>


namespace cmvr
{

/**
 * @brief 单次点到点运动的 S 曲线剖面参数
 */
struct SCurveProfile
{
  // 7 个阶段的持续时间
  double t1;  // 加加速度上升时间（加速阶段）
  double t2;  // 匀加速时间
  double t3;  // 加加速度下降时间（加速阶段结束）
  double t4;  // 匀速时间（巡航）
  double t5;  // 加加速度下降时间（减速阶段）
  double t6;  // 匀减速时间
  double t7;  // 加加速度上升时间（减速阶段结束）

  double total_time;

  // 运动约束
  double j_max;   // 最大加加速度 (rad/s³)
  double a_max;   // 最大加速度 (rad/s²)
  double v_max;   // 最大速度 (rad/s)

  // 运动参数
  double distance;      // 运动总距离
  double direction;     // +1 或 -1
  double v_cruise;      // 实际达到的巡航速度
  double a_limit;       // 实际达到的加速度上限

  // 初始条件
  double p0;  // 初始位置
  double v0;  // 初始速度
  double a0;  // 初始加速度
  double vf;  // 终止速度

  SCurveProfile()
    : t1(0), t2(0), t3(0), t4(0), t5(0), t6(0), t7(0)
    , total_time(0)
    , j_max(50.0), a_max(10.0), v_max(3.0)
    , distance(0), direction(1.0), v_cruise(0), a_limit(0)
    , p0(0), v0(0), a0(0), vf(0)
  {}
};

/**
 * @brief 严格点到点位置 S 曲线规划器
 */
class SCurve
{
public:
  /**
   * @brief 构造函数（带运动约束）
   * @param max_velocity 最大速度 (rad/s)
   * @param max_acceleration 最大加速度 (rad/s²)
   * @param max_jerk 最大加加速度 (rad/s³)
   */
  SCurve(double max_velocity = 3.0,
                  double max_acceleration = 10.0,
                  double max_jerk = 50.0);

  /**
   * @brief 设置运动约束
   */
  void setConstraints(double max_velocity, double max_acceleration, double max_jerk);

  /**
   * @brief 获取当前运动约束
   */
  void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;

  /**
   * @brief 计算点到点运动的完整 S 曲线剖面
   * @param start_position 起始位置
   * @param end_position 终止位置
   * @param start_velocity 起始速度（默认 0）
   * @param end_velocity 终止速度（默认 0）
   * @return 计算得到的剖面参数
   */
  SCurveProfile calculateProfile(double start_position, double end_position,
                                  double start_velocity = 0.0, double end_velocity = 0.0);

  /**
   * @brief 给定剖面下，获取时刻 t 的位置
   * @param profile S 曲线剖面
   * @param t 从起点开始的时间
   * @return 时刻 t 的位置
   */
  double getPositionAtTime(const SCurveProfile& profile, double t) const;

  /**
   * @brief 给定剖面下，获取时刻 t 的速度
   * @param profile S 曲线剖面
   * @param t 从起点开始的时间
   * @return 时刻 t 的速度
   */
  double getVelocityAtTime(const SCurveProfile& profile, double t) const;

  /**
   * @brief 给定剖面下，获取时刻 t 的加速度
   * @param profile S 曲线剖面
   * @param t 从起点开始的时间
   * @return 时刻 t 的加速度
   */
  double getAccelerationAtTime(const SCurveProfile& profile, double t) const;

  /**
   * @brief 给定剖面下，获取时刻 t 的加加速度
   * @param profile S 曲线剖面
   * @param t 从起点开始的时间
   * @return 时刻 t 的加加速度
   */
  double getJerkAtTime(const SCurveProfile& profile, double t) const;

  /**
   * @brief 按固定时间间隔生成轨迹点
   * @param profile S 曲线剖面
   * @param dt 轨迹点时间间隔
   * @param positions 输出：每个时间点的位置
   * @param velocities 输出：每个时间点的速度
   * @param accelerations 输出：每个时间点的加速度
   */
  void generateTrajectory(const SCurveProfile& profile, double dt,
                          std::vector<double>& positions,
                          std::vector<double>& velocities,
                          std::vector<double>& accelerations) const;

private:
  double max_velocity_;
  double max_acceleration_;
  double max_jerk_;

  static constexpr double EPSILON = 1e-9;
  static constexpr double VELOCITY_THRESHOLD = 1e-6;

  double computeSegmentJerk(const SCurveProfile& profile, double t) const;
  void calculateShortProfile(SCurveProfile& profile) const;
  void calculateLongProfile(SCurveProfile& profile) const;
};

}  // namespace cmvr