Branch data Line data Source code
1 : : // Copyright 2020 Mapless AI, Inc.
2 : : //
3 : : // Permission is hereby granted, free of charge, to any person obtaining a copy
4 : : // of this software and associated documentation files (the "Software"), to
5 : : // deal in the Software without restriction, including without limitation the
6 : : // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 : : // sell copies of the Software, and to permit persons to whom the Software is
8 : : // furnished to do so, subject to the following conditions:
9 : : //
10 : : // The above copyright notice and this permission notice shall be included in
11 : : // all copies or substantial portions of the Software.
12 : : //
13 : : // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 : : // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 : : // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 : : // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 : : // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 : : // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 : : // IN THE SOFTWARE.
20 : :
21 : : #ifndef GEOMETRY__INTERVAL_HPP_
22 : : #define GEOMETRY__INTERVAL_HPP_
23 : :
24 : : #include <algorithm>
25 : : #include <cmath>
26 : : #include <iostream>
27 : : #include <limits>
28 : : #include <sstream>
29 : : #include <stdexcept>
30 : : #include <string>
31 : : #include <type_traits>
32 : : #include <utility>
33 : :
34 : : #include "common/types.hpp"
35 : : #include "helper_functions/float_comparisons.hpp"
36 : :
37 : : namespace autoware
38 : : {
39 : : namespace common
40 : : {
41 : : namespace geometry
42 : : {
43 : :
44 : : /**
45 : : * @brief Data structure to contain scalar interval bounds.
46 : : *
47 : : * @post The interval is guaranteed to be valid upon successful construction. An
48 : : * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds
49 : : * are ordered (min <= max).
50 : : */
51 : : template<typename T>
52 : : class Interval
53 : : {
54 : : static_assert(
55 : : std::is_floating_point<T>::value,
56 : : "Intervals only support floating point types.");
57 : :
58 : : public:
59 : : /**
60 : : * @brief Compute equality.
61 : : *
62 : : * Two intervals compare equal iff they are both valid and they are both
63 : : * either empty or have equal bounds.
64 : : *
65 : : * @note This is defined inline because the class is templated.
66 : : *
67 : : * @return True iff the intervals compare equal.
68 : : */
69 : 12 : friend bool operator==(const Interval & i1, const Interval & i2)
70 : : {
71 : 12 : const auto min_eq = (Interval::min(i1) == Interval::min(i2));
72 : 12 : const auto max_eq = (Interval::max(i1) == Interval::max(i2));
73 [ + + + + ]: 12 : const auto bounds_equal = (min_eq && max_eq);
74 [ + + + - ]: 12 : const auto both_empty = (Interval::empty(i1) && Interval::empty(i2));
75 [ + + + + ]: 12 : return both_empty || bounds_equal;
76 : : }
77 : :
78 : : /**
79 : : * @brief Compute inequality and the logical negation of equality.
80 : : * @note This is defined inline because the class is templated.
81 : : */
82 : 6 : friend bool operator!=(const Interval & i1, const Interval & i2)
83 : : {
84 : 6 : return !(i1 == i2);
85 : : }
86 : :
87 : : /**
88 : : * @brief Stream overload for Interval types.
89 : : *
90 : : * @note Output precision is fixed inside the function definition, and the
91 : : * serialization is JSON compatible.
92 : : *
93 : : * @note The serialization is lossy. It is used for debugging and for
94 : : * generating exception strings.
95 : : */
96 : 2 : friend std::ostream & operator<<(std::ostream & os, const Interval & i)
97 : : {
98 : 2 : constexpr auto PRECISION = 5;
99 : :
100 : : // Internal helper to format the output.
101 : 4 : const auto readable_extremum = [](const T & val) {
102 [ - + # # ]: 4 : if (val == std::numeric_limits<T>::lowest()) {
103 [ # # # # ]: 0 : return std::string("REAL_MIN");
104 : : }
105 [ - + # # ]: 4 : if (val == std::numeric_limits<T>::max()) {
106 [ # # # # ]: 0 : return std::string("REAL_MAX");
107 : : }
108 : :
109 [ + - # # ]: 8 : std::stringstream ss;
110 : 4 : ss.setf(std::ios::fixed, std::ios::floatfield);
111 : 4 : ss.precision(PRECISION);
112 [ + - # # ]: 4 : ss << val;
113 [ + - # # ]: 4 : return ss.str();
114 : : };
115 : :
116 : : // Do stream output.
117 [ - + ]: 2 : if (Interval::empty(i)) {
118 [ # # ]: 0 : return os << "{}";
119 : : }
120 : 0 : return os << "{\"min\": " << readable_extremum(Interval::min(i)) <<
121 [ + - + - : 2 : ", \"max\": " << readable_extremum(Interval::max(i)) << "}";
+ - + - +
- + - +
- ]
122 : : }
123 : :
124 : : /**
125 : : * @brief Test whether the two intervals have bounds within epsilon of each
126 : : * other.
127 : : *
128 : : * @note If both intervals are empty, this returns true. If only one is empty,
129 : : * this returns false.
130 : : */
131 : : static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps);
132 : :
133 : : /** @brief The minimum bound of the interval. */
134 : 1337 : static T min(const Interval & i) {return i.min_;}
135 : :
136 : : /** @brief The maximum bound of the interval. */
137 : 940 : static T max(const Interval & i) {return i.max_;}
138 : :
139 : : /**
140 : : * @brief Return the measure (length) of the interval.
141 : : *
142 : : * @note For empty or invalid intervals, NaN is returned. See Interval::empty
143 : : * for note on distinction between measure zero and the emptiness property.
144 : : */
145 : : static T measure(const Interval & i);
146 : :
147 : : /**
148 : : * @brief Utility for checking whether an interval has zero measure.
149 : : *
150 : : * @note For empty or invalid intervals, false is returned. See
151 : : * Interval::empty for note on distinction between measure zero and the
152 : : * emptiness property.
153 : : *
154 : : * @return True iff the interval has zero measure.
155 : : */
156 : : static bool zero_measure(const Interval & i);
157 : :
158 : : /**
159 : : * @brief Whether the interval is empty or not.
160 : : *
161 : : * @note Emptiness refers to whether the interval contains any points and is
162 : : * thus a distinct property from measure: an interval is non-empty if contains
163 : : * only a single point even though its measure in that case is zero.
164 : : *
165 : : * @return True iff the interval is empty.
166 : : */
167 : : static bool empty(const Interval & i);
168 : :
169 : : /**
170 : : * @brief Test for whether a given interval contains a given value within the given epsilon
171 : : * @return True iff 'interval' contains 'value'.
172 : : */
173 : : static bool contains(
174 : : const Interval & i, const T & value,
175 : : const T & eps = std::numeric_limits<T>::epsilon());
176 : :
177 : : /**
178 : : * @brief Test for whether 'i1' subseteq 'i2'
179 : : * @return True iff i1 subseteq i2.
180 : : */
181 : : static bool is_subset_eq(const Interval & i1, const Interval & i2);
182 : :
183 : : /**
184 : : * @brief Compute the intersection of two intervals as a new interval.
185 : : */
186 : : static Interval intersect(const Interval & i1, const Interval & i2);
187 : :
188 : : /**
189 : : * @brief Clamp a scalar 'val' onto 'interval'.
190 : : * @return If 'val' in 'interval', return 'val'; otherwise return the nearer
191 : : * interval bound.
192 : : */
193 : : static T clamp_to(const Interval & i, T val);
194 : :
195 : : /**
196 : : * @brief Constructor: initialize an empty interval with members set to NaN.
197 : : */
198 : : Interval();
199 : :
200 : : /**
201 : : * @brief Constructor: specify exact interval bounds.
202 : : *
203 : : * @note An empty interval is specified by setting both bounds to NaN.
204 : : * @note An exception is thrown if the specified bounds are invalid.
205 : : *
206 : : * @post Construction is successful iff the interval is valid.
207 : : */
208 : : Interval(const T & min, const T & max);
209 : :
210 : : private:
211 : : static constexpr T NaN = std::numeric_limits<T>::quiet_NaN();
212 : :
213 : : T min_;
214 : : T max_;
215 : :
216 : : /**
217 : : * @brief Verify that the bounds are valid in an interval.
218 : : * @note This method is private because it can only be used in the
219 : : * constructor. Once an interval has been constructed, its bounds are
220 : : * guaranteed to be valid.
221 : : */
222 : : static bool bounds_valid(const Interval & i);
223 : : }; // class Interval
224 : :
225 : : //------------------------------------------------------------------------------
226 : :
227 : : typedef Interval<autoware::common::types::float64_t> Interval_d;
228 : : typedef Interval<autoware::common::types::float32_t> Interval_f;
229 : :
230 : : //------------------------------------------------------------------------------
231 : :
232 : : template<typename T>
233 : : constexpr T Interval<T>::NaN;
234 : :
235 : : //------------------------------------------------------------------------------
236 : :
237 : : template<typename T>
238 : 16 : Interval<T>::Interval()
239 : 16 : : min_(Interval::NaN), max_(Interval::NaN) {}
240 : :
241 : : //------------------------------------------------------------------------------
242 : :
243 : : template<typename T>
244 : 297 : Interval<T>::Interval(const T & min, const T & max)
245 : 297 : : min_(min), max_(max)
246 : : {
247 [ + + ]: 297 : if (!Interval::bounds_valid(*this)) {
248 [ + - ]: 4 : std::stringstream ss;
249 [ + - + - ]: 2 : ss << "Attempted to construct an invalid interval: " << *this;
250 [ + - + - ]: 2 : throw std::runtime_error(ss.str());
251 : : }
252 : 295 : }
253 : :
254 : : //------------------------------------------------------------------------------
255 : :
256 : : template<typename T>
257 : 8 : bool Interval<T>::abs_eq(
258 : : const Interval & i1, const Interval & i2, const T & eps)
259 : : {
260 : : namespace comp = helper_functions::comparisons;
261 : :
262 [ + + + + ]: 8 : const auto both_empty = Interval::empty(i1) && Interval::empty(i2);
263 [ + + + + ]: 8 : const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2);
264 : :
265 : 8 : const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps);
266 : 8 : const auto maxs_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps);
267 [ + + + + : 8 : const auto both_non_empty_equal = both_non_empty && mins_equal && maxs_equal;
+ - ]
268 : :
269 [ + + + + ]: 8 : return both_empty || both_non_empty_equal;
270 : : }
271 : :
272 : : //------------------------------------------------------------------------------
273 : :
274 : : template<typename T>
275 : 3 : bool Interval<T>::zero_measure(const Interval & i)
276 : : {
277 : : // An empty interval will return false because (NaN == NaN) is false.
278 : 3 : return Interval::min(i) == Interval::max(i);
279 : : }
280 : :
281 : : //------------------------------------------------------------------------------
282 : :
283 : : template<typename T>
284 : 377 : bool Interval<T>::empty(const Interval & i)
285 : : {
286 [ + + + - ]: 377 : return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i));
287 : : }
288 : :
289 : : //------------------------------------------------------------------------------
290 : :
291 : : template<typename T>
292 : 297 : bool Interval<T>::bounds_valid(const Interval & i)
293 : : {
294 : 297 : const auto is_ordered = (Interval::min(i) <= Interval::max(i));
295 : :
296 : : // Check for emptiness expicitly because it requires both bounds to be NaN
297 [ + - + + ]: 297 : return Interval::empty(i) || is_ordered;
298 : : }
299 : :
300 : : //------------------------------------------------------------------------------
301 : :
302 : : template<typename T>
303 : 5 : bool Interval<T>::is_subset_eq(const Interval & i1, const Interval & i2)
304 : : {
305 : 5 : const auto lower_contained = (Interval::min(i1) >= Interval::min(i2));
306 : 5 : const auto upper_contained = (Interval::max(i1) <= Interval::max(i2));
307 [ + + + - ]: 5 : return lower_contained && upper_contained;
308 : : }
309 : :
310 : : //------------------------------------------------------------------------------
311 : :
312 : : template<typename T>
313 : 195 : bool Interval<T>::contains(const Interval & i, const T & value, const T & eps)
314 : : {
315 [ + - + + : 354 : return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) &&
+ + ]
316 [ + - + + ]: 354 : common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps);
317 : : }
318 : :
319 : : //------------------------------------------------------------------------------
320 : :
321 : : template<typename T>
322 : 5 : T Interval<T>::measure(const Interval & i)
323 : : {
324 : 5 : return Interval::max(i) - Interval::min(i);
325 : : }
326 : :
327 : : //------------------------------------------------------------------------------
328 : :
329 : : template<typename T>
330 : 4 : Interval<T> Interval<T>::intersect(const Interval & i1, const Interval & i2)
331 : : {
332 : : // Construct intersection assuming an intersection exists.
333 : : try {
334 [ + + - + ]: 4 : const auto either_empty = Interval::empty(i1) || Interval::empty(i2);
335 : 4 : const auto min = std::max(Interval::min(i1), Interval::min(i2));
336 : 4 : const auto max = std::min(Interval::max(i1), Interval::max(i2));
337 [ + + + + ]: 4 : return either_empty ? Interval() : Interval(min, max);
338 : 1 : } catch (...) {
339 : : }
340 : :
341 : : // Otherwise, the intersection is empty.
342 : 1 : return Interval();
343 : : }
344 : :
345 : : //------------------------------------------------------------------------------
346 : :
347 : : template<typename T>
348 : 30 : T Interval<T>::clamp_to(const Interval & i, T val)
349 : : {
350 : : // clamp val to min
351 : 30 : val = std::max(val, Interval::min(i));
352 : :
353 : : // clamp val to max
354 : 30 : val = std::min(val, Interval::max(i));
355 : :
356 [ + + ]: 30 : return Interval::empty(i) ? Interval::NaN : val;
357 : : }
358 : :
359 : : //------------------------------------------------------------------------------
360 : :
361 : : } // namespace geometry
362 : : } // namespace common
363 : : } // namespace autoware
364 : :
365 : : #endif // GEOMETRY__INTERVAL_HPP_
|
