CanonicalAngle.hpp

#pragma once
 
#include <frc/geometry/Rotation2d.h>
#include <units/angle.h>
#include <cassert>
 
namespace thunder {
namespace types {
 
class CanonicalAngle {
  double m_cos = 1.0;
  double m_sin = 0.0;
 
 public:
  CanonicalAngle() = default;
 
  /*implicit*/ CanonicalAngle(const frc::Rotation2d& rotation)
      : m_cos(rotation.Cos()), m_sin(rotation.Sin()) {}
 
  /*implicit*/ CanonicalAngle(units::angle_unit auto value)  // NOLINT
      : m_cos{gcem::cos(value.template convert<units::radian>().value())},
        m_sin{gcem::sin(value.template convert<units::radian>().value())} {}
 
  CanonicalAngle(double x, double y) {
    double magnitude = gcem::hypot(x, y);
    assert((magnitude > 1e-6) && "Cannot create CanonicalAngle with zero magnitude");
    m_cos = x / magnitude;
    m_sin = y / magnitude;
  }
 
  double cos() const { return m_cos; }
  double sin() const { return m_sin; }
  double tan() const { return m_sin / m_cos; }
 
  units::radian_t radians() const { return units::radian_t{gcem::atan2(m_sin, m_cos)}; }
 
  units::degree_t degrees() const { return radians().convert<units::degree>(); }
 
  /*implicit*/ operator frc::Rotation2d() const { return frc::Rotation2d{m_cos, m_sin}; }
 
  /*implicit*/ operator units::angle_unit auto() const { return radians(); }
 
  bool operator==(const CanonicalAngle& other) const {
    return gcem::hypot(cos() - other.cos(), sin() - other.sin()) < 1E-9;
  }
 
  bool operator!=(const CanonicalAngle& other) const { return !(*this == other); }
 
  CanonicalAngle operator-() const { return CanonicalAngle{-radians()}; }
 
  CanonicalAngle operator+(const CanonicalAngle& other) const { return rotateBy(other); }
 
  CanonicalAngle operator-(const CanonicalAngle& other) const { return rotateBy(-other); }
 
  CanonicalAngle operator*(double scalar) const { return CanonicalAngle(radians() * scalar); }
 
  CanonicalAngle operator/(double scalar) const { return CanonicalAngle(radians() / scalar); }
 
  CanonicalAngle rotateBy(const CanonicalAngle& other) const {
    return {cos() * other.cos() - sin() * other.sin(), cos() * other.sin() + sin() * other.cos()};
  }
 
  CanonicalAngle supplementary() const { return CanonicalAngle{-m_cos, -m_sin}; }
 
  bool isSupplementaryTo(const CanonicalAngle& other) const { return supplementary() == other; }
};
 
}  // namespace types
 
using types::CanonicalAngle;
 
}  // namespace thunder