Program Listing for File Modular.hpp

↰ Return to documentation for file (/tmp/ws/src/rmf_utils/rmf_utils/include/rmf_utils/Modular.hpp)

/*
 * Copyright (C) 2020 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef RMF_UTILS__MODULAR_HPP
#define RMF_UTILS__MODULAR_HPP

#include <stdexcept>
#include <limits>
#include <type_traits>

namespace rmf_utils {

//==============================================================================
template<typename V>
class Modular
{
public:

  Modular(V basis)
  : _basis(basis)
  {
    // Do nothing
  }

  bool less_than(const V rhs) const
  {
    // "distance" here means how far the RHS is from the basis, adjusted back by
    // half of the maximum window size.
    const V distance = rhs - _basis + HalfWindow;

    // If the distance from the adjusted basis is greater than the maximum
    // window size, then we have an exception.
    if (Window < distance)
    {
      throw std::runtime_error(
              "[rmf_traffic::schedule::Modular] modular distance between value ["
              + std::to_string(
                rhs) + "] and basis [" + std::to_string(_basis)
              + "] is too big [" + std::to_string(distance) + "]. Maximum is "
              + std::to_string(Window));
    }

    // If the distance from the adjusted basis is less than half the window
    // size, then RHS is below (i.e. less than) the original basis. Otherwise it
    // is not less than the original basis.
    return HalfWindow < distance;
  }

  bool less_than_or_equal(const V rhs) const
  {
    return (_basis == rhs) || less_than(rhs);
  }

  bool less_than(const V lhs, const V rhs) const
  {
    return (lhs - _basis) < (rhs - _basis);
  }

  bool less_than_or_equal(const V lhs, const V rhs) const
  {
    return (lhs == rhs) || less_than(lhs, rhs);
  }

private:

  constexpr static V Window =
    static_cast<V>(std::numeric_limits<std::make_signed_t<V>>::max());

  constexpr static V HalfWindow = Window/2;

  V _basis = 0;

};

template<typename V>
Modular<V> modular(V value)
{
  return Modular<V>(value);
}

template<typename V>
struct ModularLess
{
  bool operator()(const V& lhs, const V& rhs) const
  {
    return modular(lhs).less_than(rhs);
  }
};

} // namespace rmf_utils

#endif // RMF_UTILS__MODULAR_HPP