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 : 281 : static T min(const Interval & i) {return i.min_;}
135 : :
136 : : /** @brief The maximum bound of the interval. */
137 : 169 : 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.
171 : : * @return True iff 'interval' contains 'value'.
172 : : */
173 : : static bool contains(const Interval & i, const T & value);
174 : :
175 : : /**
176 : : * @brief Test for whether 'i1' subseteq 'i2'
177 : : * @return True iff i1 subseteq i2.
178 : : */
179 : : static bool is_subset_eq(const Interval & i1, const Interval & i2);
180 : :
181 : : /**
182 : : * @brief Compute the intersection of two intervals as a new interval.
183 : : */
184 : : static Interval intersect(const Interval & i1, const Interval & i2);
185 : :
186 : : /**
187 : : * @brief Clamp a scalar 'val' onto 'interval'.
188 : : * @return If 'val' in 'interval', return 'val'; otherwise return the nearer
189 : : * interval bound.
190 : : */
191 : : static T clamp_to(const Interval & i, T val);
192 : :
193 : : /**
194 : : * @brief Constructor: initialize an empty interval with members set to NaN.
195 : : */
196 : : Interval();
197 : :
198 : : /**
199 : : * @brief Constructor: specify exact interval bounds.
200 : : *
201 : : * @note An empty interval is specified by setting both bounds to NaN.
202 : : * @note An exception is thrown if the specified bounds are invalid.
203 : : *
204 : : * @post Construction is successful iff the interval is valid.
205 : : */
206 : : Interval(const T & min, const T & max);
207 : :
208 : : private:
209 : : static constexpr T NaN = std::numeric_limits<T>::quiet_NaN();
210 : :
211 : : T min_;
212 : : T max_;
213 : :
214 : : /**
215 : : * @brief Verify that the bounds are valid in an interval.
216 : : * @note This method is private because it can only be used in the
217 : : * constructor. Once an interval has been constructed, its bounds are
218 : : * guaranteed to be valid.
219 : : */
220 : : static bool bounds_valid(const Interval & i);
221 : : }; // class Interval
222 : :
223 : : //------------------------------------------------------------------------------
224 : :
225 : : typedef Interval<autoware::common::types::float64_t> Interval_d;
226 : : typedef Interval<autoware::common::types::float32_t> Interval_f;
227 : :
228 : : //------------------------------------------------------------------------------
229 : :
230 : : template<typename T>
231 : : constexpr T Interval<T>::NaN;
232 : :
233 : : //------------------------------------------------------------------------------
234 : :
235 : : template<typename T>
236 : 16 : Interval<T>::Interval()
237 : 16 : : min_(Interval::NaN), max_(Interval::NaN) {}
238 : :
239 : : //------------------------------------------------------------------------------
240 : :
241 : : template<typename T>
242 : 47 : Interval<T>::Interval(const T & min, const T & max)
243 : 47 : : min_(min), max_(max)
244 : : {
245 [ + + ]: 47 : if (!Interval::bounds_valid(*this)) {
246 [ + - ]: 4 : std::stringstream ss;
247 [ + - + - ]: 2 : ss << "Attempted to construct an invalid interval: " << *this;
248 [ + - + - ]: 2 : throw std::runtime_error(ss.str());
249 : : }
250 : 45 : }
251 : :
252 : : //------------------------------------------------------------------------------
253 : :
254 : : template<typename T>
255 : 8 : bool Interval<T>::abs_eq(
256 : : const Interval & i1, const Interval & i2, const T & eps)
257 : : {
258 : : namespace comp = helper_functions::comparisons;
259 : :
260 [ + + + + ]: 8 : const auto both_empty = Interval::empty(i1) && Interval::empty(i2);
261 [ + + + + ]: 8 : const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2);
262 : :
263 : 8 : const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps);
264 : 8 : const auto maxs_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps);
265 [ + + + + : 8 : const auto both_non_empty_equal = both_non_empty && mins_equal && maxs_equal;
+ - ]
266 : :
267 [ + + + + ]: 8 : return both_empty || both_non_empty_equal;
268 : : }
269 : :
270 : : //------------------------------------------------------------------------------
271 : :
272 : : template<typename T>
273 : 3 : bool Interval<T>::zero_measure(const Interval & i)
274 : : {
275 : : // An empty interval will return false because (NaN == NaN) is false.
276 : 3 : return Interval::min(i) == Interval::max(i);
277 : : }
278 : :
279 : : //------------------------------------------------------------------------------
280 : :
281 : : template<typename T>
282 : 127 : bool Interval<T>::empty(const Interval & i)
283 : : {
284 [ + + + - ]: 127 : return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i));
285 : : }
286 : :
287 : : //------------------------------------------------------------------------------
288 : :
289 : : template<typename T>
290 : 47 : bool Interval<T>::bounds_valid(const Interval & i)
291 : : {
292 : 47 : const auto is_ordered = (Interval::min(i) <= Interval::max(i));
293 : :
294 : : // Check for emptiness expicitly because it requires both bounds to be NaN
295 [ + - + + ]: 47 : return Interval::empty(i) || is_ordered;
296 : : }
297 : :
298 : : //------------------------------------------------------------------------------
299 : :
300 : : template<typename T>
301 : 5 : bool Interval<T>::is_subset_eq(const Interval & i1, const Interval & i2)
302 : : {
303 : 5 : const auto lower_contained = (Interval::min(i1) >= Interval::min(i2));
304 : 5 : const auto upper_contained = (Interval::max(i1) <= Interval::max(i2));
305 [ + + + - ]: 5 : return lower_contained && upper_contained;
306 : : }
307 : :
308 : : //------------------------------------------------------------------------------
309 : :
310 : : template<typename T>
311 : 3 : bool Interval<T>::contains(const Interval & i, const T & value)
312 : : {
313 [ + + + + ]: 3 : return (value >= Interval::min(i)) && (value <= Interval::max(i));
314 : : }
315 : :
316 : : //------------------------------------------------------------------------------
317 : :
318 : : template<typename T>
319 : 5 : T Interval<T>::measure(const Interval & i)
320 : : {
321 : 5 : return Interval::max(i) - Interval::min(i);
322 : : }
323 : :
324 : : //------------------------------------------------------------------------------
325 : :
326 : : template<typename T>
327 : 4 : Interval<T> Interval<T>::intersect(const Interval & i1, const Interval & i2)
328 : : {
329 : : // Construct intersection assuming an intersection exists.
330 : : try {
331 [ + + - + ]: 4 : const auto either_empty = Interval::empty(i1) || Interval::empty(i2);
332 : 4 : const auto min = std::max(Interval::min(i1), Interval::min(i2));
333 : 4 : const auto max = std::min(Interval::max(i1), Interval::max(i2));
334 [ + + + + ]: 4 : return either_empty ? Interval() : Interval(min, max);
335 : 1 : } catch (...) {
336 : : }
337 : :
338 : : // Otherwise, the intersection is empty.
339 : 1 : return Interval();
340 : : }
341 : :
342 : : //------------------------------------------------------------------------------
343 : :
344 : : template<typename T>
345 : 30 : T Interval<T>::clamp_to(const Interval & i, T val)
346 : : {
347 : : // clamp val to min
348 : 30 : val = std::max(val, Interval::min(i));
349 : :
350 : : // clamp val to max
351 : 30 : val = std::min(val, Interval::max(i));
352 : :
353 [ + + ]: 30 : return Interval::empty(i) ? Interval::NaN : val;
354 : : }
355 : :
356 : : //------------------------------------------------------------------------------
357 : :
358 : : } // namespace geometry
359 : : } // namespace common
360 : : } // namespace autoware
361 : :
362 : : #endif // GEOMETRY__INTERVAL_HPP_
|
