rotation2d.h

#pragma once
#undef __ARM_NEON__
#undef __ARM_NEON
#include <Eigen/Dense>
 
#include "core/utils/math/geometry/translation2d.h"
#include <cmath>
#include <iostream>
#include <vector>
 
#ifndef PI
#define PI 3.141592654
#endif
 
class Translation2d;

class Rotation2d {
  public:
    constexpr Rotation2d() : m_radians(0), m_cos(1), m_sin(0) {};
    Rotation2d(const double &radians);

    Rotation2d(const double &x, const double &y);

    Rotation2d(const Translation2d &translation);

    double radians() const;

    double degrees() const;

    double revolutions() const;

    double f_cos() const;

    double f_sin() const;

    double f_tan() const;

    Eigen::Matrix2d rotation_matrix() const;

    double wrapped_radians_180() const;

    double wrapped_degrees_180() const;

    double wrapped_revolutions_180() const;

    double wrapped_radians_360() const;

    double wrapped_degrees_360() const;

    double wrapped_revolutions_360() const;

    Rotation2d operator+(const Rotation2d &other) const;

    Rotation2d operator-(const Rotation2d &other) const;

    Rotation2d operator-() const;

    Rotation2d operator*(const double &scalar) const;

    Rotation2d operator/(const double &scalar) const;

    bool operator==(const Rotation2d &other) const;

    friend std::ostream &operator<<(std::ostream &os, const Rotation2d &rotation);
 
  private:
    double m_radians;
    double m_cos;
    double m_sin;
};
 
// functions that don't belong in the class because they're useful elsewhere
Rotation2d from_radians(const double &radians);

Rotation2d from_degrees(const double &degrees);

Rotation2d from_revolutions(const double &revolutions);

double wrap_radians_180(const double &angle);

double wrap_degrees_180(const double &angle);

double wrap_revolutions_180(const double &angle);

double wrap_radians_360(const double &angle);

double wrap_degrees_360(const double &angle);

double wrap_revolutions_360(const double &angle);

double deg2rad(double deg);

double rad2deg(double r);

Rotation2d unwrapped_mean(const std::vector<Rotation2d> &list);

Rotation2d wrapped_mean(const std::vector<Rotation2d> &list);