hermite_point.h

#pragma once
 
#include "core/math/geometry/pose2d.h"
#include "core/math/geometry/rotation2d.h"
#include "core/math/geometry/translation2d.h"
#include "core/math/units.h"
 
using namespace units::literals;

struct HermitePoint {
  Translation2d point;              
  Translation2d tangent;            
  Translation2d second_derivative;  

  HermitePoint() = default;

  HermitePoint(const Translation2d& point, const Translation2d& tangent,
               const Translation2d& second_derivative = Translation2d())
      : point(point), tangent(tangent), second_derivative(second_derivative) {}

  HermitePoint(double x, double y, double vx, double vy)
      : point(x * 1_in, y * 1_in), tangent(vx * 1_in, vy * 1_in), second_derivative(0_in, 0_in) {}

  HermitePoint(double x, double y, double vx, double vy, double ax, double ay)
      : point(x * 1_in, y * 1_in),
        tangent(vx * 1_in, vy * 1_in),
        second_derivative(ax * 1_in, ay * 1_in) {}

  static HermitePoint from_derivatives(const Translation2d& point, const Translation2d& tangent,
                                       const Translation2d& second_derivative = Translation2d()) {
    return HermitePoint(point, tangent, second_derivative);
  }

  static HermitePoint from_heading(units::Length x, units::Length y, Rotation2d heading,
                                   double speed, Rotation2d accel_heading = Rotation2d(),
                                   double accel_mag = 0.0) {
    return HermitePoint(Translation2d(x, y), Translation2d(speed * 1_in, heading),
                        Translation2d(accel_mag * 1_in, accel_heading));
  }

  static HermitePoint from_heading(const Translation2d& point, Rotation2d heading, double speed,
                                   Rotation2d accel_heading = Rotation2d(),
                                   double accel_mag = 0.0) {
    return HermitePoint(point, Translation2d(speed * 1_in, heading),
                        Translation2d(accel_mag * 1_in, accel_heading));
  }

  static HermitePoint from_pose(const Pose2d& pose, double speed, double accel_mag = 0.0) {
    return HermitePoint(pose.translation(), Translation2d(speed * 1_in, pose.rotation()),
                        Translation2d(accel_mag * 1_in, pose.rotation()));
  }

  Translation2d get_point() const { return point; }
  Translation2d get_tangent() const { return tangent; }
  Translation2d get_second_derivative() const { return second_derivative; }
};