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 HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_
22 : : #define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_
23 : :
24 : : #include <algorithm>
25 : : #include <cmath>
26 : : #include <limits>
27 : :
28 : : namespace autoware
29 : : {
30 : : namespace common
31 : : {
32 : : namespace helper_functions
33 : : {
34 : : namespace comparisons
35 : : {
36 : :
37 : : /**
38 : : * @brief Check for approximate equality in absolute terms.
39 : : * @pre eps >= 0
40 : : * @return True iff 'a' and 'b' are within 'eps' of each other.
41 : : */
42 : : template<typename T>
43 : 56 : bool abs_eq(const T & a, const T & b, const T & eps)
44 : : {
45 : : static_assert(
46 : : std::is_floating_point<T>::value,
47 : : "Float comparisons only support floating point types.");
48 : :
49 : 56 : return std::abs(a - b) <= eps;
50 : : }
51 : :
52 : : /**
53 : : * @brief Check for approximate less than in absolute terms.
54 : : * @pre eps >= 0
55 : : * @return True iff 'a' is less than 'b' minus 'eps'.
56 : : */
57 : : template<typename T>
58 : 13 : bool abs_lt(const T & a, const T & b, const T & eps)
59 : : {
60 [ + + + + ]: 13 : return !abs_eq(a, b, eps) && (a < b);
61 : : }
62 : :
63 : : /**
64 : : * @brief Check for approximate less than or equal in absolute terms.
65 : : * @pre eps >= 0
66 : : * @return True iff 'a' is less than or equal to 'b' plus 'eps'.
67 : : */
68 : : template<typename T>
69 : 16 : bool abs_lte(const T & a, const T & b, const T & eps)
70 : : {
71 [ + + + + ]: 16 : return abs_eq(a, b, eps) || (a < b);
72 : : }
73 : :
74 : : /**
75 : : * @brief Check for approximate greater than or equal in absolute terms.
76 : : * @pre eps >= 0
77 : : * @return True iff 'a' is greater than or equal to 'b' minus 'eps'.
78 : : */
79 : : template<typename T>
80 : 8 : bool abs_gte(const T & a, const T & b, const T & eps)
81 : : {
82 : 8 : return !abs_lt(a, b, eps);
83 : : }
84 : :
85 : : /**
86 : : * @brief Check for approximate greater than in absolute terms.
87 : : * @pre eps >= 0
88 : : * @return True iff 'a' is greater than 'b' minus 'eps'.
89 : : */
90 : : template<typename T>
91 : 6 : bool abs_gt(const T & a, const T & b, const T & eps)
92 : : {
93 : 6 : return !abs_lte(a, b, eps);
94 : : }
95 : :
96 : : /**
97 : : * @brief Check whether a value is within epsilon of zero.
98 : : * @pre eps >= 0
99 : : * @return True iff 'a' is within 'eps' of zero.
100 : : */
101 : : template<typename T>
102 : 5 : bool abs_eq_zero(const T & a, const T & eps)
103 : : {
104 : 5 : return abs_eq(a, static_cast<T>(0), eps);
105 : : }
106 : :
107 : : /**
108 : : * @brief
109 : : * https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
110 : : * @pre rel_eps >= 0
111 : : * @return True iff 'a' and 'b' are within relative 'rel_eps' of each other.
112 : : */
113 : : template<typename T>
114 : 23 : bool rel_eq(const T & a, const T & b, const T & rel_eps)
115 : : {
116 : : static_assert(
117 : : std::is_floating_point<T>::value,
118 : : "Float comparisons only support floating point types.");
119 : :
120 : 23 : const auto delta = std::abs(a - b);
121 : 23 : const auto larger = std::max(std::abs(a), std::abs(b));
122 : 23 : const auto max_rel_delta = (larger * rel_eps);
123 : 23 : return delta <= max_rel_delta;
124 : : }
125 : :
126 : : // TODO(jeff): As needed, add relative variants of <, <=, >, >=
127 : :
128 : : /**
129 : : * @brief Check for approximate equality in absolute and relative terms.
130 : : *
131 : : * @note This method should be used only if an explicit relative or absolute
132 : : * comparison is not appropriate for the particular use case.
133 : : *
134 : : * @pre abs_eps >= 0
135 : : * @pre rel_eps >= 0
136 : : * @return True iff 'a' and 'b' are within 'eps' or 'rel_eps' of each other
137 : : */
138 : : template<typename T>
139 : 14 : bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps)
140 : : {
141 : 14 : const auto are_absolute_eq = abs_eq(a, b, abs_eps);
142 : 14 : const auto are_relative_eq = rel_eq(a, b, rel_eps);
143 [ + + + + ]: 14 : return are_absolute_eq || are_relative_eq;
144 : : }
145 : :
146 : : } // namespace comparisons
147 : : } // namespace helper_functions
148 : : } // namespace common
149 : : } // namespace autoware
150 : :
151 : : #endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_
|
