comp_type.hpp

#pragma once
 
#include "bsp.hpp"
 
namespace Type {
 
class CycleValue {
 public:
  CycleValue& operator=(const CycleValue&) = default;
 
  static float Calculate(float value) {
    value = std::fmodf(value, M_2PI);
    if (value < 0) {
      value += M_2PI;
    }
    return value;
  }
 
  CycleValue(const float& value) : value_(Calculate(value)) {}
 
  CycleValue(const double& value)
      : value_(Calculate(static_cast<float>(value))) {}
 
  CycleValue(const CycleValue& value) : value_(value.value_) {
    while (value_ >= M_2PI) {
      value_ -= M_2PI;
    }
 
    while (value_ < 0) {
      value_ += M_2PI;
    }
  }
 
  CycleValue() : value_(0.0f) {}
 
  CycleValue operator+(const float& value) {
    return CycleValue(value + value_);
  }
 
  CycleValue operator+(const double& value) {
    return CycleValue(static_cast<float>(value) + value_);
  }
 
  CycleValue operator+(const CycleValue& value) {
    return CycleValue(value.value_ + value_);
  }
 
  CycleValue operator+=(const float& value) {
    value_ = Calculate(value + value_);
    return *this;
  }
 
  CycleValue operator+=(const double& value) {
    value_ = Calculate(static_cast<float>(value) + value_);
    return *this;
  }
 
  CycleValue operator+=(const CycleValue& value) {
    value_ = Calculate(value.value_ + value_);
    return *this;
  }
 
  float operator-(const float& raw_value) {
    float value = Calculate(raw_value);
    float ans = value_ - value;
    while (ans >= M_PI) {
      ans -= M_2PI;
    }
 
    while (ans < -M_PI) {
      ans += M_2PI;
    }
 
    return ans;
  }
 
  float operator-(const double& raw_value) {
    float value = Calculate(static_cast<float>(raw_value));
    float ans = value_ - value;
    while (ans >= M_PI) {
      ans -= M_2PI;
    }
 
    while (ans < -M_PI) {
      ans += M_2PI;
    }
 
    return ans;
  }
 
  float operator-(const CycleValue& value) {
    float ans = value_ - value.value_;
    while (ans >= M_PI) {
      ans -= M_2PI;
    }
 
    while (ans < -M_PI) {
      ans += M_2PI;
    }
 
    return ans;
  }
 
  CycleValue operator-() { return CycleValue(M_2PI - value_); }
 
  operator float() { return this->value_; }
 
  CycleValue& operator=(const float& value) {
    value_ = Calculate(value);
    return *this;
  }
 
  CycleValue& operator=(const double& value) {
    value_ = Calculate(static_cast<float>(value));
    return *this;
  }
 
  float Value() { return value_; }
 
 private:
  float value_; /* The normalized cyclic value in [0, 2π). */
};
 
typedef struct {
  CycleValue yaw;
  CycleValue pit;
  CycleValue rol;
} Eulr;
 
typedef struct {
  float q0;
  float q1;
  float q2;
  float q3;
} Quaternion;
 
typedef struct {
  float x;
  float y;
} Vector2;
 
typedef struct {
  float x;
  float y;
  float z;
} Vector3;
 
};  // namespace Type