cyclic.h

Go to the documentation of this file.

// clang: MatousFormat
#ifndef CYCLIC_H
#define CYCLIC_H
 
#include <cmath>
#include <ostream>
 
namespace mrs_lib
{
  namespace geometry
  {
    template <typename flt, class spec>
    struct cyclic
    {
      cyclic() : val(minimum){};
      cyclic(const flt val) : val(wrap(val)){};
      cyclic(const cyclic& other) : val(other.val){};
      cyclic(const spec& other) : val(other.val){};
 
      flt value() const
      {
        return val;
      };
 
      static constexpr flt minimum = spec::minimum;   
      static constexpr flt supremum = spec::supremum; 
      static constexpr flt range =
          supremum - minimum; 
      static constexpr flt half_range =
          range / flt(2); 
      /* static_assert((supremum > minimum), "cyclic value: Range not valid"); */
 
      static bool inRange(const flt val)
      {
        return val >= minimum && val < supremum;
      }
 
      static flt wrap(const flt val)
      {
        // these few ifs should cover most cases, improving speed and precision
        if (val >= minimum)
        {
          if (val < supremum)  // value is actually in range and doesn't need to be wrapped
            return val;
          else if (val < supremum + range)
            return val - range;  // to avoid unnecessary costly fmod operation for this case (assumed to be significantly more common than the general case)
        } else
        {
          if (val >= minimum - range)
            return val + range;  // to avoid unnecessary costly fmod operation for this case (assumed to be significantly more common than the general case)
        }
 
        // general case
        const flt rem = std::fmod(val - minimum, range);
        const flt wrapped = rem + minimum + std::signbit(rem) * range;
        return wrapped;
      }
 
      static flt unwrap(const flt what, const flt from)
      {
        return from + diff(what, from);
      }
 
      static flt pdist(const flt from, const flt to)
      {
        return pdist(cyclic(from), cyclic(to));
      }
 
      static flt pdist(const cyclic from, const cyclic to)
      {
        const flt tmp = to.val - from.val;
        const flt dist = tmp + std::signbit(tmp) * range;
        return dist;
      }
 
      static flt diff(const flt minuend, const flt subtrahend)
      {
        return diff(cyclic(minuend), cyclic(subtrahend));
      }
 
      static flt diff(const cyclic minuend, const cyclic subtrahend)
      {
        const flt d = minuend.val - subtrahend.val;
        if (d < -half_range)
          return d + range;
        if (d >= half_range)
          return d - range;
        return d;
      }
 
      static flt dist(const flt from, const flt to)
      {
        return dist(cyclic(from), cyclic(to));
      }
 
      static flt dist(const cyclic from, const cyclic to)
      {
        return std::abs(diff(from, to));
      }
 
      static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
      {
        return interpUnwrapped(cyclic(from), cyclic(to), coeff);
      }
 
      static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
      {
        const flt dang = diff(to, from);
        const flt intp = from.val + coeff * dang;
        return intp;
      }
 
      static flt interp(const flt from, const flt to, const flt coeff)
      {
        return wrap(interpUnwrapped(from, to, coeff));
      }
 
      static flt interp(const cyclic from, const cyclic to, const flt coeff)
      {
        return wrap(interpUnwrapped(from, to, coeff));
      }
 
      static flt pinterpUnwrapped(const flt from, const flt to, const flt coeff)
      {
        return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
      }
 
      static flt pinterpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
      {
        const flt dang = pdist(to, from);
        const flt intp = from.val + coeff * dang;
        return intp;
      }
 
      static flt pinterp(const flt from, const flt to, const flt coeff)
      {
        return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
      }
 
      static flt pinterp(const cyclic from, const cyclic to, const flt coeff)
      {
        return wrap(pinterpUnwrapped(from, to, coeff));
      }
 
      template <class other_t>
      static other_t convert(const cyclic& what)
      {
        return other_t(what.val / range * other_t::range);
      }
 
      template <class other_t>
      other_t convert() const
      {
        return other_t(val / range * other_t::range);
      }
 
      // | ------------------------ Operators ----------------------- |
      cyclic& operator=(const flt nval)
      {
        val = wrap(nval);
        return *this;
      };
      cyclic& operator=(const cyclic& other)
      {
        val = other.val;
        return *this;
      };
      cyclic& operator=(cyclic&& other)
      {
        val = other.val;
        return *this;
      };
 
      cyclic& operator+=(const cyclic& other)
      {
        val = wrap(val + other.val);
        return *this;
      };
 
      cyclic& operator-=(const cyclic& other)
      {
        val = diff(val, other.val);
        return *this;
      };
 
      friend spec operator+(const cyclic& lhs, const cyclic& rhs)
      {
        return wrap(lhs.val + rhs.val);
      }
 
      friend flt operator-(const cyclic& lhs, const cyclic& rhs)
      {
        return diff(lhs, rhs);
      }
 
      protected:
        flt val;
      };
 
    template <typename flt, class spec>
    bool operator<(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
    {
      return cyclic<flt, spec>::dist(lhs, 0) < cyclic<flt, spec>::dist(rhs, 0);
    }
 
    template <typename flt, class spec>
    bool operator>(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
    {
      return cyclic<flt, spec>::dist(lhs, 0) > cyclic<flt, spec>::dist(rhs, 0);
    }
 
    template <typename flt, class spec>
    std::ostream& operator<<(std::ostream &out, const cyclic<flt, spec>& ang)
    {
      return (out << ang.value());
    }
 
    struct radians : public cyclic<double, radians>
    {
      using cyclic<double, radians>::cyclic;  // necessary to inherit constructors
      static constexpr double minimum = 0;
      static constexpr double supremum = 2 * M_PI;
    };
 
    struct sradians : public cyclic<double, sradians>
    {
      using cyclic<double, sradians>::cyclic;  // necessary to inherit constructors
      static constexpr double minimum = -M_PI;
      static constexpr double supremum = M_PI;
    };
 
    struct degrees : public cyclic<double, degrees>
    {
      using cyclic<double, degrees>::cyclic;  // necessary to inherit constructors
      static constexpr double minimum = 0;
      static constexpr double supremum = 360;
    };
 
    struct sdegrees : public cyclic<double, sdegrees>
    {
      using cyclic<double, sdegrees>::cyclic;  // necessary to inherit constructors
      static constexpr double minimum = -180;
      static constexpr double supremum = 180;
    };
  }  // namespace geometry
}  // namespace mrs_lib
 
#endif  // CYCLIC_H