CoordinateUnit.h

Go to the documentation of this file.

#ifndef COORDINATE_UNIT_H_
#define COORDINATE_UNIT_H_
#include <chrono>
#include <cmath>
 
#include "bits/float_cmp.h"
 
namespace rct {
 
 
 
template<int N>
struct CoordinateUnit {
  float x_milli;  
  float y_milli;  
  float ang_rad;  
 
  static constexpr auto dimension() noexcept {
    return N;
  }
 
  CoordinateUnit& operator+=(const CoordinateUnit& obj) noexcept {
    this->x_milli += obj.x_milli;
    this->y_milli += obj.y_milli;
    this->ang_rad += obj.ang_rad;
    return *this;
  }
  CoordinateUnit& operator-=(const CoordinateUnit& obj) noexcept {
    this->x_milli -= obj.x_milli;
    this->y_milli -= obj.y_milli;
    this->ang_rad -= obj.ang_rad;
    return *this;
  }
  CoordinateUnit& operator*=(const float obj) noexcept {
    this->x_milli *= obj;
    this->y_milli *= obj;
    this->ang_rad *= obj;
    return *this;
  }
  CoordinateUnit& operator/=(const float obj) noexcept {
    this->x_milli /= obj;
    this->y_milli /= obj;
    this->ang_rad /= obj;
    return *this;
  }
};
 
using Coordinate = CoordinateUnit<0>;
using Velocity = CoordinateUnit<-1>;
 
template<int M, int N>
CoordinateUnit<M> unit_cast(const CoordinateUnit<N>& obj) {
  return *reinterpret_cast<const CoordinateUnit<M>*>(&obj);
}
 
constexpr float distance(const Coordinate& p1, const Coordinate& p2) {
  return std::hypot(p1.x_milli - p2.x_milli, p1.y_milli - p2.y_milli);
}
 
template<class T>
constexpr T lerp(const T& a, const T& b, float t) noexcept {
  return a + t * (b - a);
}
 
template<int N>
CoordinateUnit<N> operator+(const CoordinateUnit<N>& lhs, const CoordinateUnit<N>& rhs) {
  CoordinateUnit<N> nrv{lhs};
  nrv += rhs;
  return nrv;
};
template<int N>
CoordinateUnit<N> operator-(const CoordinateUnit<N>& lhs, const CoordinateUnit<N>& rhs) {
  CoordinateUnit<N> nrv{lhs};
  nrv -= rhs;
  return nrv;
};
template<int N>
CoordinateUnit<N> operator*(const CoordinateUnit<N>& lhs, const float rhs) {
  auto nrv{lhs};
  nrv *= rhs;
  return nrv;
};
template<int N>
CoordinateUnit<N> operator*(const float lhs, const CoordinateUnit<N>& rhs) {
  return rhs * lhs;
};
template<int N>
CoordinateUnit<N> operator/(const CoordinateUnit<N>& lhs, const float rhs) {
  auto nrv{lhs};
  nrv /= rhs;
  return nrv;
};
template<int N>
CoordinateUnit<N + 1> operator*(const CoordinateUnit<N>& obj, const std::chrono::microseconds& sec) {
  return unit_cast<N + 1>(obj * sec.count() * 1e-6);
}
template<int N>
CoordinateUnit<N + 1> operator*(const std::chrono::microseconds& sec, const CoordinateUnit<N>& obj) {
  return obj * sec;
}
template<int N>
CoordinateUnit<N - 1> operator/(const CoordinateUnit<N>& obj, const std::chrono::microseconds& sec) {
  return unit_cast<N - 1>(obj / sec.count() * 1e6);
}
template<int N>
bool operator==(const CoordinateUnit<N>& lhs, const CoordinateUnit<N>& rhs) {
  return impl::float_cmp(lhs.x_milli, rhs.x_milli) == 0 && impl::float_cmp(lhs.y_milli, rhs.y_milli) == 0 &&
         impl::float_cmp(lhs.ang_rad, rhs.ang_rad) == 0;
}
template<int N>
bool operator!=(const CoordinateUnit<N>& lhs, const CoordinateUnit<N>& rhs) {
  return !(lhs == rhs);
}
 
 
 
}  // namespace rct
 
#endif  // COORDINATE_UNIT_H_