Add Interval class (gazebosim#388)

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

examples/CMakeLists.txt

@@ -24,6 +24,9 @@ target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math$
 add_executable(helpers_example helpers_example.cc)
 target_link_libraries(helpers_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 
+add_executable(interval_example interval_example.cc)
+target_link_libraries(interval_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
+
 add_executable(kmeans kmeans.cc)
 target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 

examples/interval_example.cc

@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2022 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.
+ *
+*/
+//! [complete]
+#include <iostream>
+#include <ignition/math/Interval.hh>
+
+int main(int argc, char **argv)
+{
+  std::cout << std::boolalpha;
+
+  const ignition::math::Intervald defaultInterval;
+  // A default constructed interval should be empty.
+  std::cout << "The " << defaultInterval << " interval is empty: "
+            << defaultInterval.Empty() << std::endl;
+
+  const ignition::math::Intervald openInterval =
+      ignition::math::Intervald::Open(-1., 1.);
+  // An open interval should exclude its endpoints.
+  std::cout << "The " << openInterval << " interval contains its endpoints: "
+            << (openInterval.Contains(openInterval.LeftValue()) ||
+                openInterval.Contains(openInterval.RightValue()))
+            << std::endl;
+
+  const ignition::math::Intervald closedInterval =
+      ignition::math::Intervald::Closed(0., 1.);
+
+  // A closed interval should include its endpoints.
+  std::cout << "The " << closedInterval << " interval contains its endpoints: "
+            << (closedInterval.Contains(closedInterval.LeftValue()) ||
+                closedInterval.Contains(closedInterval.RightValue()))
+            << std::endl;
+
+  // Closed and open intervals may intersect.
+  std::cout << "Intervals " << closedInterval << " and " << openInterval
+            << " intersect: " << closedInterval.Intersects(openInterval)
+            << std::endl;
+
+  // The unbounded interval should include all non-empty intervals.
+  std::cout << "The " << ignition::math::Intervald::Unbounded
+            << " interval contains all previous non-empty intervals: "
+            << (ignition::math::Intervald::Unbounded.Contains(openInterval) ||
+                ignition::math::Intervald::Unbounded.Contains(closedInterval))
+            << std::endl;
+
+}
+//! [complete]

include/ignition/math/Interval.hh

@@ -0,0 +1,292 @@
+/*
+ * Copyright (C) 2022 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 IGNITION_MATH_INTERVAL_HH_
+#define IGNITION_MATH_INTERVAL_HH_
+
+#include <cmath>
+#include <limits>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+
+#include <ignition/math/config.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
+    //
+    /// \class Interval Interval.hh ignition/math/Interval.hh
+    /// \brief The Interval class represents a range of real numbers.
+    /// Intervals may be open (a, b), left-closed [a, b), right-closed
+    /// (a, b], or fully closed [a, b].
+    ///
+    /// ## Example
+    ///
+    /// \snippet examples/interval_example.cc complete
+    template <typename T>
+    class Interval
+    {
+      /// \brief An unbounded interval (-∞, ∞)
+      public: static const Interval<T> &Unbounded;
+
+      /// \brief Constructor
+      public: Interval() = default;
+
+      /// \brief Constructor
+      /// \param[in] _leftValue leftmost interval value
+      /// \param[in] _leftClosed whether the interval is left-closed or not
+      /// \param[in] _rightValue rightmost interval value
+      /// \param[in] _rightClosed whether the interval is right-closed or not
+      public: Interval(T _leftValue, bool _leftClosed,
+                       T _rightValue, bool _rightClosed)
+      : leftValue(std::move(_leftValue)),
+        rightValue(std::move(_rightValue)),
+        leftClosed(_leftClosed),
+        rightClosed(_rightClosed)
+      {
+      }
+
+      /// \brief Make an open interval (`_leftValue`, `_rightValue`)
+      /// \param[in] _leftValue leftmost interval value
+      /// \param[in] _rightValue rightmost interval value
+      /// \return the open interval
+      public: static Interval<T> Open(T _leftValue, T _rightValue)
+      {
+        return Interval<T>(
+          std::move(_leftValue), false,
+          std::move(_rightValue), false);
+      }
+
+      /// \brief Make a left-closed interval [`_leftValue`, `_rightValue`)
+      /// \param[in] _leftValue leftmost interval value
+      /// \param[in] _rightValue rightmost interval value
+      /// \return the left-closed interval
+      public: static Interval<T> LeftClosed(T _leftValue, T _rightValue)
+      {
+        return Interval<T>(
+          std::move(_leftValue), true,
+          std::move(_rightValue), false);
+      }
+
+      /// \brief Make a right-closed interval (`_leftValue`, `_rightValue`]
+      /// \param[in] _leftValue leftmost interval value
+      /// \param[in] _rightValue rightmost interval value
+      /// \return the left-closed interval
+      public: static Interval<T> RightClosed(T _leftValue, T _rightValue)
+      {
+        return Interval<T>(
+          std::move(_leftValue), false,
+          std::move(_rightValue), true);
+      }
+
+      /// \brief Make a closed interval [`_leftValue`, `_rightValue`]
+      /// \param[in] _leftValue leftmost interval value
+      /// \param[in] _rightValue rightmost interval value
+      /// \return the closed interval
+      public: static Interval<T> Closed(T _leftValue, T _rightValue)
+      {
+        return Interval<T>{
+          std::move(_leftValue), true,
+          std::move(_rightValue), true};
+      }
+
+      /// \brief Get the leftmost interval value
+      /// \return the leftmost interval value
+      public: const T &LeftValue() const { return this->leftValue; }
+
+      /// \brief Check if the interval is left-closed
+      /// \return true if the interval is left-closed, false otherwise
+      public: bool IsLeftClosed() const { return this->leftClosed; }
+
+      /// \brief Get the rightmost interval value
+      /// \return the rightmost interval value
+      public: const T &RightValue() const { return this->rightValue; }
+
+      /// \brief Check if the interval is right-closed
+      /// \return true if the interval is right-closed, false otherwise
+      public: bool IsRightClosed() const { return this->rightClosed; }
+
+      /// \brief Check if the interval is empty
+      /// Some examples of empty intervals include
+      /// (a, a), [a, a), and [a + 1, a].
+      /// \return true if it is empty, false otherwise
+      public: bool Empty() const
+      {
+        if (this->leftClosed && this->rightClosed)
+        {
+          return this->rightValue < this->leftValue;
+        }
+        return this->rightValue <= this->leftValue;
+      }
+
+      /// \brief Check if the interval contains `_value`
+      /// \param[in] _value value to check for membership
+      /// \return true if it is contained, false otherwise
+      public: bool Contains(const T &_value) const
+      {
+        if (this->leftClosed && this->rightClosed)
+        {
+          return this->leftValue <= _value && _value <= this->rightValue;
+        }
+        if (this->leftClosed)
+        {
+          return this->leftValue <= _value && _value < this->rightValue;
+        }
+        if (this->rightClosed)
+        {
+          return this->leftValue < _value && _value <= this->rightValue;
+        }
+        return this->leftValue < _value && _value < this->rightValue;
+      }
+
+      /// \brief Check if the interval contains `_other` interval
+      /// \param[in] _other interval to check for membership
+      /// \return true if it is contained, false otherwise
+      public: bool Contains(const Interval<T> &_other) const
+      {
+        if (this->Empty() || _other.Empty())
+        {
+          return false;
+        }
+        if (!this->leftClosed && _other.leftClosed)
+        {
+          if (_other.leftValue <= this->leftValue)
+          {
+            return false;
+          }
+        }
+        else
+        {
+          if (_other.leftValue < this->leftValue)
+          {
+            return false;
+          }
+        }
+        if (!this->rightClosed && _other.rightClosed)
+        {
+          if (this->rightValue <= _other.rightValue)
+          {
+            return false;
+          }
+        }
+        else
+        {
+          if (this->rightValue < _other.rightValue)
+          {
+            return false;
+          }
+        }
+        return true;
+      }
+
+      /// \brief Check if the interval intersects `_other` interval
+      /// \param[in] _other interval to check for intersection
+      /// \return true if both intervals intersect, false otherwise
+      public: bool Intersects(const Interval<T> &_other) const
+      {
+        if (this->Empty() || _other.Empty())
+        {
+          return false;
+        }
+        if (this->rightClosed && _other.leftClosed)
+        {
+          if (this->rightValue < _other.leftValue)
+          {
+            return false;
+          }
+        }
+        else
+        {
+          if (this->rightValue <= _other.leftValue)
+          {
+            return false;
+          }
+        }
+        if (_other.rightClosed && this->leftClosed)
+        {
+          if (_other.rightValue < this->leftValue)
+          {
+            return false;
+          }
+        }
+        else
+        {
+          if (_other.rightValue <= this->leftValue)
+          {
+            return false;
+          }
+        }
+        return true;
+      }
+
+      /// \brief Equality test operator
+      /// \param _other interval to check for equality
+      /// \return true if intervals are equal, false otherwise
+      public: bool operator==(const Interval<T> &_other) const
+      {
+        return this->Contains(_other) && _other.Contains(*this);
+      }
+
+      /// \brief Inequality test operator
+      /// \param _other interval to check for inequality
+      /// \return true if intervals are unequal, false otherwise
+      public: bool operator!=(const Interval<T> &_other) const
+      {
+        return !this->Contains(_other) || !_other.Contains(*this);
+      }
+
+      /// \brief Stream insertion operator
+      /// \param _out output stream
+      /// \param _interval Interval to output
+      /// \return the stream
+      public: friend std::ostream &operator<<(
+        std::ostream &_out, const ignition::math::Interval<T> &_interval)
+      {
+        return _out << (_interval.leftClosed ? "[" : "(")
+                    << _interval.leftValue << ", " << _interval.rightValue
+                    << (_interval.rightClosed ? "]" : ")");
+      }
+
+      /// \brief The leftmost interval value
+      private: T leftValue{0};
+      /// \brief The righmost interval value
+      private: T rightValue{0};
+      /// \brief Whether the interval is left-closed or not
+      private: bool leftClosed{false};
+      /// \brief Whether the interval is right-closed or not
+      private: bool rightClosed{false};
+    };
+
+    namespace detail {
+       template<typename T>
+       const Interval<T> gUnboundedInterval =
+           Interval<T>::Open(-std::numeric_limits<T>::infinity(),
+                             std::numeric_limits<T>::infinity());
+    }  // namespace detail
+    template<typename T>
+    const Interval<T> &Interval<T>::Unbounded = detail::gUnboundedInterval<T>;
+
+    using Intervalf = Interval<float>;
+    using Intervald = Interval<double>;
+    }
+  }
+}
+
+#endif