angle.h

#pragma once
 
#include <cmath>
#include <ostream>
 
#include "software/geom/geom_constants.h"
 
class Angle final
{
   public:
    static constexpr Angle zero();
 
    static constexpr Angle quarter();
 
    static constexpr Angle half();
 
    static constexpr Angle threeQuarter();
 
    static constexpr Angle full();
 
    static constexpr Angle fromRadians(double rad);
 
    static constexpr Angle fromDegrees(double deg);
 
    static Angle asin(double x);
 
    static Angle acos(double x);
 
    static Angle atan(double x);
 
    explicit constexpr Angle();
 
    constexpr double toRadians() const;
 
    constexpr double toDegrees() const;
 
    constexpr Angle mod(Angle divisor) const;
 
    constexpr Angle remainder(const Angle &divisor) const;
 
    constexpr Angle abs() const;
 
    bool isFinite() const;
 
    double sin() const;
 
    double cos() const;
 
    double tan() const;
 
    constexpr Angle clamp() const;
 
    constexpr Angle minDiff(const Angle &other) const;
 
   private:
    double rads;
 
    explicit constexpr Angle(double rads);
};
 
constexpr Angle operator-(const Angle &angle) __attribute__((warn_unused_result));
 
constexpr Angle operator+(const Angle &x, const Angle &y)
    __attribute__((warn_unused_result));
 
constexpr Angle operator-(const Angle &x, const Angle &y)
    __attribute__((warn_unused_result));
 
constexpr Angle operator*(const Angle &angle, double scale)
    __attribute__((warn_unused_result));
 
constexpr Angle operator*(double scale, const Angle &angle)
    __attribute__((warn_unused_result));
 
constexpr Angle operator/(const Angle &angle, double divisor)
    __attribute__((warn_unused_result));
 
constexpr double operator/(const Angle &x, const Angle &y)
    __attribute__((warn_unused_result));
 
Angle &operator+=(Angle &x, const Angle &y);
 
Angle &operator-=(Angle &x, const Angle &y);
 
Angle &operator*=(Angle &angle, double scale);
 
Angle &operator/=(Angle &angle, double divisor);
 
constexpr bool operator<(const Angle &x, const Angle &y);
 
constexpr bool operator>(const Angle &x, const Angle &y);
 
constexpr bool operator<=(const Angle &x, const Angle &y);
 
constexpr bool operator>=(const Angle &x, const Angle &y);
 
bool operator==(const Angle &x, const Angle &y);
 
constexpr bool operator!=(const Angle &x, const Angle &y);
 
inline std::ostream &operator<<(std::ostream &os, const Angle &a);
 
inline constexpr Angle Angle::zero()
{
    return Angle();
}
 
inline constexpr Angle Angle::quarter()
{
    return Angle(M_PI / 2.0);
}
 
inline constexpr Angle Angle::half()
{
    return Angle(M_PI);
}
 
inline constexpr Angle Angle::threeQuarter()
{
    return Angle(3.0 / 2.0 * M_PI);
}
 
inline constexpr Angle Angle::full()
{
    return Angle(2.0 * M_PI);
}
 
inline constexpr Angle Angle::fromRadians(double rad)
{
    return Angle(rad);
}
 
inline constexpr Angle Angle::fromDegrees(double deg)
{
    return Angle(deg / 180.0 * M_PI);
}
 
inline Angle Angle::asin(double x)
{
    return Angle::fromRadians(std::asin(x));
}
 
inline Angle Angle::acos(double x)
{
    return fromRadians(std::acos(x));
}
 
inline Angle Angle::atan(double x)
{
    return Angle::fromRadians(std::atan(x));
}
 
inline constexpr Angle::Angle() : rads(0.0) {}
 
inline constexpr double Angle::toRadians() const
{
    return rads;
}
 
inline constexpr double Angle::toDegrees() const
{
    return rads / M_PI * 180.0;
}
 
inline constexpr Angle Angle::mod(Angle divisor) const
{
    if (divisor.toRadians() == 0)
    {
        return Angle::fromRadians(toRadians());
    }
    else
    {
        return Angle::fromRadians(toRadians() - static_cast<double>(static_cast<long>(
                                                    toRadians() / divisor.toRadians())) *
                                                    divisor.toRadians());
    }
}
 
inline constexpr Angle Angle::remainder(const Angle &divisor) const
{
    return Angle::fromRadians(toRadians() -
                              static_cast<double>(static_cast<long>(
                                  (toRadians() / divisor.toRadians()) >= 0
                                      ? (toRadians() / divisor.toRadians() + 0.5)
                                      : (toRadians() / divisor.toRadians() - 0.5))) *
                                  divisor.toRadians());
}
 
inline constexpr Angle Angle::abs() const
{
    return Angle::fromRadians(toRadians() < 0 ? -toRadians() : toRadians());
}
 
inline bool Angle::isFinite() const
{
    return std::isfinite(toRadians());
}
 
inline double Angle::sin() const
{
    return std::sin(toRadians());
}
 
inline double Angle::cos() const
{
    return std::cos(toRadians());
}
 
inline double Angle::tan() const
{
    return std::tan(toRadians());
}
 
inline constexpr Angle Angle::clamp() const
{
    return remainder(Angle::full());
}
 
inline constexpr Angle Angle::minDiff(const Angle &other) const
{
    return (*this - other).clamp().abs();
}
 
inline constexpr Angle::Angle(double rads) : rads(rads) {}
 
inline constexpr Angle operator-(const Angle &angle)
{
    return Angle::fromRadians(-angle.toRadians());
}
 
inline constexpr Angle operator+(const Angle &x, const Angle &y)
{
    return Angle::fromRadians(x.toRadians() + y.toRadians());
}
 
inline constexpr Angle operator-(const Angle &x, const Angle &y)
{
    return Angle::fromRadians(x.toRadians() - y.toRadians());
}
 
inline constexpr Angle operator*(const Angle &angle, double scale)
{
    return Angle::fromRadians(angle.toRadians() * scale);
}
 
inline constexpr Angle operator*(double scale, const Angle &angle)
{
    return Angle::fromRadians(scale * angle.toRadians());
}
 
inline constexpr Angle operator/(const Angle &angle, double divisor)
{
    return Angle::fromRadians(angle.toRadians() / divisor);
}
 
inline constexpr double operator/(const Angle &x, const Angle &y)
{
    return x.toRadians() / y.toRadians();
}
 
inline Angle &operator+=(Angle &x, const Angle &y)
{
    return x = x + y;
}
 
inline Angle &operator-=(Angle &x, const Angle &y)
{
    return x = x - y;
}
 
inline Angle &operator*=(Angle &angle, double scale)
{
    return angle = angle * scale;
}
 
inline Angle &operator/=(Angle &angle, double divisor)
{
    return angle = angle / divisor;
}
 
inline constexpr bool operator<(const Angle &x, const Angle &y)
{
    return x.toRadians() < y.toRadians();
}
 
inline constexpr bool operator>(const Angle &x, const Angle &y)
{
    return x.toRadians() > y.toRadians();
}
 
inline constexpr bool operator<=(const Angle &x, const Angle &y)
{
    return x.toRadians() <= y.toRadians();
}
 
inline constexpr bool operator>=(const Angle &x, const Angle &y)
{
    return x.toRadians() >= y.toRadians();
}
 
inline bool operator==(const Angle &x, const Angle &y)
{
    Angle diff = x.clamp().minDiff(y.clamp());
    return diff.toRadians() <= FIXED_EPSILON;
}
 
inline constexpr bool operator!=(const Angle &x, const Angle &y)
{
    return x.toRadians() != y.toRadians();
}
 
inline std::ostream &operator<<(std::ostream &os, const Angle &a)
{
    os << a.toRadians() << "R";
    return os;
}