|
| 1 | +/* |
| 2 | + * Copyright (C) 2022 Open Source Robotics Foundation |
| 3 | + * |
| 4 | + * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | + * you may not use this file except in compliance with the License. |
| 6 | + * You may obtain a copy of the License at |
| 7 | + * |
| 8 | + * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | + * |
| 10 | + * Unless required by applicable law or agreed to in writing, software |
| 11 | + * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | + * See the License for the specific language governing permissions and |
| 14 | + * limitations under the License. |
| 15 | + * |
| 16 | +*/ |
| 17 | +#ifndef IGNITION_MATH_INTERVAL_HH_ |
| 18 | +#define IGNITION_MATH_INTERVAL_HH_ |
| 19 | + |
| 20 | +#include <cmath> |
| 21 | +#include <limits> |
| 22 | +#include <ostream> |
| 23 | +#include <type_traits> |
| 24 | +#include <utility> |
| 25 | + |
| 26 | +#include <ignition/math/config.hh> |
| 27 | + |
| 28 | +namespace ignition |
| 29 | +{ |
| 30 | + namespace math |
| 31 | + { |
| 32 | + // Inline bracket to help doxygen filtering. |
| 33 | + inline namespace IGNITION_MATH_VERSION_NAMESPACE { |
| 34 | + // |
| 35 | + /// \class Interval Interval.hh ignition/math/Interval.hh |
| 36 | + /// \brief The Interval class represents a range of real numbers. |
| 37 | + /// Intervals may be open (a, b), left-closed [a, b), right-closed |
| 38 | + /// (a, b], or fully closed [a, b]. |
| 39 | + /// |
| 40 | + /// ## Example |
| 41 | + /// |
| 42 | + /// \snippet examples/interval_example.cc complete |
| 43 | + template <typename T> |
| 44 | + class Interval |
| 45 | + { |
| 46 | + /// \brief An unbounded interval (-∞, ∞) |
| 47 | + public: static const Interval<T> &Unbounded; |
| 48 | + |
| 49 | + /// \brief Constructor |
| 50 | + public: Interval() = default; |
| 51 | + |
| 52 | + /// \brief Constructor |
| 53 | + /// \param[in] _leftValue leftmost interval value |
| 54 | + /// \param[in] _leftClosed whether the interval is left-closed or not |
| 55 | + /// \param[in] _rightValue rightmost interval value |
| 56 | + /// \param[in] _rightClosed whether the interval is right-closed or not |
| 57 | + public: Interval(T _leftValue, bool _leftClosed, |
| 58 | + T _rightValue, bool _rightClosed) |
| 59 | + : leftValue(std::move(_leftValue)), |
| 60 | + rightValue(std::move(_rightValue)), |
| 61 | + leftClosed(_leftClosed), |
| 62 | + rightClosed(_rightClosed) |
| 63 | + { |
| 64 | + } |
| 65 | + |
| 66 | + /// \brief Make an open interval (`_leftValue`, `_rightValue`) |
| 67 | + /// \param[in] _leftValue leftmost interval value |
| 68 | + /// \param[in] _rightValue rightmost interval value |
| 69 | + /// \return the open interval |
| 70 | + public: static Interval<T> Open(T _leftValue, T _rightValue) |
| 71 | + { |
| 72 | + return Interval<T>( |
| 73 | + std::move(_leftValue), false, |
| 74 | + std::move(_rightValue), false); |
| 75 | + } |
| 76 | + |
| 77 | + /// \brief Make a left-closed interval [`_leftValue`, `_rightValue`) |
| 78 | + /// \param[in] _leftValue leftmost interval value |
| 79 | + /// \param[in] _rightValue rightmost interval value |
| 80 | + /// \return the left-closed interval |
| 81 | + public: static Interval<T> LeftClosed(T _leftValue, T _rightValue) |
| 82 | + { |
| 83 | + return Interval<T>( |
| 84 | + std::move(_leftValue), true, |
| 85 | + std::move(_rightValue), false); |
| 86 | + } |
| 87 | + |
| 88 | + /// \brief Make a right-closed interval (`_leftValue`, `_rightValue`] |
| 89 | + /// \param[in] _leftValue leftmost interval value |
| 90 | + /// \param[in] _rightValue rightmost interval value |
| 91 | + /// \return the left-closed interval |
| 92 | + public: static Interval<T> RightClosed(T _leftValue, T _rightValue) |
| 93 | + { |
| 94 | + return Interval<T>( |
| 95 | + std::move(_leftValue), false, |
| 96 | + std::move(_rightValue), true); |
| 97 | + } |
| 98 | + |
| 99 | + /// \brief Make a closed interval [`_leftValue`, `_rightValue`] |
| 100 | + /// \param[in] _leftValue leftmost interval value |
| 101 | + /// \param[in] _rightValue rightmost interval value |
| 102 | + /// \return the closed interval |
| 103 | + public: static Interval<T> Closed(T _leftValue, T _rightValue) |
| 104 | + { |
| 105 | + return Interval<T>{ |
| 106 | + std::move(_leftValue), true, |
| 107 | + std::move(_rightValue), true}; |
| 108 | + } |
| 109 | + |
| 110 | + /// \brief Get the leftmost interval value |
| 111 | + /// \return the leftmost interval value |
| 112 | + public: const T &LeftValue() const { return this->leftValue; } |
| 113 | + |
| 114 | + /// \brief Check if the interval is left-closed |
| 115 | + /// \return true if the interval is left-closed, false otherwise |
| 116 | + public: bool IsLeftClosed() const { return this->leftClosed; } |
| 117 | + |
| 118 | + /// \brief Get the rightmost interval value |
| 119 | + /// \return the rightmost interval value |
| 120 | + public: const T &RightValue() const { return this->rightValue; } |
| 121 | + |
| 122 | + /// \brief Check if the interval is right-closed |
| 123 | + /// \return true if the interval is right-closed, false otherwise |
| 124 | + public: bool IsRightClosed() const { return this->rightClosed; } |
| 125 | + |
| 126 | + /// \brief Check if the interval is empty |
| 127 | + /// Some examples of empty intervals include |
| 128 | + /// (a, a), [a, a), and [a + 1, a]. |
| 129 | + /// \return true if it is empty, false otherwise |
| 130 | + public: bool Empty() const |
| 131 | + { |
| 132 | + if (this->leftClosed && this->rightClosed) |
| 133 | + { |
| 134 | + return this->rightValue < this->leftValue; |
| 135 | + } |
| 136 | + return this->rightValue <= this->leftValue; |
| 137 | + } |
| 138 | + |
| 139 | + /// \brief Check if the interval contains `_value` |
| 140 | + /// \param[in] _value value to check for membership |
| 141 | + /// \return true if it is contained, false otherwise |
| 142 | + public: bool Contains(const T &_value) const |
| 143 | + { |
| 144 | + if (this->leftClosed && this->rightClosed) |
| 145 | + { |
| 146 | + return this->leftValue <= _value && _value <= this->rightValue; |
| 147 | + } |
| 148 | + if (this->leftClosed) |
| 149 | + { |
| 150 | + return this->leftValue <= _value && _value < this->rightValue; |
| 151 | + } |
| 152 | + if (this->rightClosed) |
| 153 | + { |
| 154 | + return this->leftValue < _value && _value <= this->rightValue; |
| 155 | + } |
| 156 | + return this->leftValue < _value && _value < this->rightValue; |
| 157 | + } |
| 158 | + |
| 159 | + /// \brief Check if the interval contains `_other` interval |
| 160 | + /// \param[in] _other interval to check for membership |
| 161 | + /// \return true if it is contained, false otherwise |
| 162 | + public: bool Contains(const Interval<T> &_other) const |
| 163 | + { |
| 164 | + if (this->Empty() || _other.Empty()) |
| 165 | + { |
| 166 | + return false; |
| 167 | + } |
| 168 | + if (!this->leftClosed && _other.leftClosed) |
| 169 | + { |
| 170 | + if (_other.leftValue <= this->leftValue) |
| 171 | + { |
| 172 | + return false; |
| 173 | + } |
| 174 | + } |
| 175 | + else |
| 176 | + { |
| 177 | + if (_other.leftValue < this->leftValue) |
| 178 | + { |
| 179 | + return false; |
| 180 | + } |
| 181 | + } |
| 182 | + if (!this->rightClosed && _other.rightClosed) |
| 183 | + { |
| 184 | + if (this->rightValue <= _other.rightValue) |
| 185 | + { |
| 186 | + return false; |
| 187 | + } |
| 188 | + } |
| 189 | + else |
| 190 | + { |
| 191 | + if (this->rightValue < _other.rightValue) |
| 192 | + { |
| 193 | + return false; |
| 194 | + } |
| 195 | + } |
| 196 | + return true; |
| 197 | + } |
| 198 | + |
| 199 | + /// \brief Check if the interval intersects `_other` interval |
| 200 | + /// \param[in] _other interval to check for intersection |
| 201 | + /// \return true if both intervals intersect, false otherwise |
| 202 | + public: bool Intersects(const Interval<T> &_other) const |
| 203 | + { |
| 204 | + if (this->Empty() || _other.Empty()) |
| 205 | + { |
| 206 | + return false; |
| 207 | + } |
| 208 | + if (this->rightClosed && _other.leftClosed) |
| 209 | + { |
| 210 | + if (this->rightValue < _other.leftValue) |
| 211 | + { |
| 212 | + return false; |
| 213 | + } |
| 214 | + } |
| 215 | + else |
| 216 | + { |
| 217 | + if (this->rightValue <= _other.leftValue) |
| 218 | + { |
| 219 | + return false; |
| 220 | + } |
| 221 | + } |
| 222 | + if (_other.rightClosed && this->leftClosed) |
| 223 | + { |
| 224 | + if (_other.rightValue < this->leftValue) |
| 225 | + { |
| 226 | + return false; |
| 227 | + } |
| 228 | + } |
| 229 | + else |
| 230 | + { |
| 231 | + if (_other.rightValue <= this->leftValue) |
| 232 | + { |
| 233 | + return false; |
| 234 | + } |
| 235 | + } |
| 236 | + return true; |
| 237 | + } |
| 238 | + |
| 239 | + /// \brief Equality test operator |
| 240 | + /// \param _other interval to check for equality |
| 241 | + /// \return true if intervals are equal, false otherwise |
| 242 | + public: bool operator==(const Interval<T> &_other) const |
| 243 | + { |
| 244 | + return this->Contains(_other) && _other.Contains(*this); |
| 245 | + } |
| 246 | + |
| 247 | + /// \brief Inequality test operator |
| 248 | + /// \param _other interval to check for inequality |
| 249 | + /// \return true if intervals are unequal, false otherwise |
| 250 | + public: bool operator!=(const Interval<T> &_other) const |
| 251 | + { |
| 252 | + return !this->Contains(_other) || !_other.Contains(*this); |
| 253 | + } |
| 254 | + |
| 255 | + /// \brief Stream insertion operator |
| 256 | + /// \param _out output stream |
| 257 | + /// \param _interval Interval to output |
| 258 | + /// \return the stream |
| 259 | + public: friend std::ostream &operator<<( |
| 260 | + std::ostream &_out, const ignition::math::Interval<T> &_interval) |
| 261 | + { |
| 262 | + return _out << (_interval.leftClosed ? "[" : "(") |
| 263 | + << _interval.leftValue << ", " << _interval.rightValue |
| 264 | + << (_interval.rightClosed ? "]" : ")"); |
| 265 | + } |
| 266 | + |
| 267 | + /// \brief The leftmost interval value |
| 268 | + private: T leftValue{0}; |
| 269 | + /// \brief The righmost interval value |
| 270 | + private: T rightValue{0}; |
| 271 | + /// \brief Whether the interval is left-closed or not |
| 272 | + private: bool leftClosed{false}; |
| 273 | + /// \brief Whether the interval is right-closed or not |
| 274 | + private: bool rightClosed{false}; |
| 275 | + }; |
| 276 | + |
| 277 | + namespace detail { |
| 278 | + template<typename T> |
| 279 | + const Interval<T> gUnboundedInterval = |
| 280 | + Interval<T>::Open(-std::numeric_limits<T>::infinity(), |
| 281 | + std::numeric_limits<T>::infinity()); |
| 282 | + } // namespace detail |
| 283 | + template<typename T> |
| 284 | + const Interval<T> &Interval<T>::Unbounded = detail::gUnboundedInterval<T>; |
| 285 | + |
| 286 | + using Intervalf = Interval<float>; |
| 287 | + using Intervald = Interval<double>; |
| 288 | + } |
| 289 | + } |
| 290 | +} |
| 291 | + |
| 292 | +#endif |
0 commit comments