chassis_accelerations.h

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

struct ChassisAccelerations {
  units::Acceleration ax = 0_inps2;

  units::Acceleration ay = 0_inps2;

  units::AngularAcceleration alpha = 0_radps2;
 
  ChassisAccelerations() = default;
 
  ChassisAccelerations(units::Acceleration ax, units::Acceleration ay,
                       units::AngularAcceleration alpha)
      : ax(ax), ay(ay), alpha(alpha) {}

  ChassisAccelerations to_robot_relative(const Rotation2d& robotAngle) const {
    Acceleration2d rotated = Acceleration2d(ax, ay).rotate_by(-robotAngle);
    return ChassisAccelerations(rotated.x(), rotated.y(), alpha);
  }

  ChassisAccelerations to_field_relative(const Rotation2d& robotAngle) const {
    Acceleration2d rotated = Acceleration2d(ax, ay).rotate_by(robotAngle);
    return ChassisAccelerations(rotated.x(), rotated.y(), alpha);
  }
 
  // Operator overloads
  ChassisAccelerations operator+(const ChassisAccelerations& other) const {
    return ChassisAccelerations(ax + other.ax, ay + other.ay, alpha + other.alpha);
  }
 
  ChassisAccelerations operator-(const ChassisAccelerations& other) const {
    return ChassisAccelerations(ax - other.ax, ay - other.ay, alpha - other.alpha);
  }
 
  ChassisAccelerations operator-() const { return ChassisAccelerations(-ax, -ay, -alpha); }
 
  ChassisAccelerations operator*(double scalar) const {
    return ChassisAccelerations(ax * scalar, ay * scalar, alpha * scalar);
  }
 
  ChassisAccelerations operator/(double scalar) const {
    return ChassisAccelerations(ax / scalar, ay / scalar, alpha / scalar);
  }
 
  bool operator==(const ChassisAccelerations& other) const {
    return ax == other.ax && ay == other.ay && alpha == other.alpha;
  }
 
  bool operator!=(const ChassisAccelerations& other) const { return !(*this == other); }
};