MRPT  1.9.9
core/include/mrpt/core/bits_math.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <algorithm> // max()
12 #include <cmath> // floor(),isnan(),...
13 #include <stdexcept>
14 
15 namespace mrpt
16 {
17 /** \addtogroup mrpt_bits_math Funtions in #include <mrpt/core/bits_math.h>
18  * \ingroup mrpt_core_grp
19  * @{ */
20 
21 /** Inline function for the square of a number. */
22 template <typename num_t, typename return_t = num_t>
23 inline return_t square(const num_t x)
24 {
25  return static_cast<return_t>(x * x);
26 }
27 
28 /** Faster version of std::hypot(), to use when overflow is not an issue and we
29  * prefer fast code. */
30 template <class T>
31 inline T hypot_fast(const T x, const T y)
32 {
33  return std::sqrt(x * x + y * y);
34 }
35 
36 #ifdef DEG2RAD // functions are preferred over macros
37 #undef DEG2RAD
38 #endif
39 #ifdef RAD2DEG
40 #undef RAD2DEG
41 #endif
42 #if !defined(M_PI)
43 #define M_PI 3.14159265358979323846
44 #endif
45 
46 /** Degrees to radians */
47 constexpr inline double DEG2RAD(const double x) { return x * M_PI / 180.0; }
48 /** Degrees to radians */
49 constexpr inline float DEG2RAD(const float x)
50 {
51  return x * float(M_PI) / 180.0f;
52 }
53 /** Degrees to radians */
54 constexpr inline double DEG2RAD(const int x) { return x * M_PI / 180.0; }
55 /** Radians to degrees */
56 constexpr inline double RAD2DEG(const double x) { return x * 180.0 / M_PI; }
57 /** Radians to degrees */
58 constexpr inline float RAD2DEG(const float x)
59 {
60  return x * 180.0f / float(M_PI);
61 }
62 #if !defined(M_PIl)
63 #define M_PIl 3.14159265358979323846264338327950288L
64 #define M_2PIl (2.0L * 3.14159265358979323846264338327950288L)
65 #endif
66 /** Degrees to radians */
67 constexpr inline long double DEG2RAD(const long double x)
68 {
69  return x * M_PIl / 180.0;
70 }
71 /** Radians to degrees */
72 constexpr inline long double RAD2DEG(const long double x)
73 {
74  return x * 180.0 / M_PIl;
75 }
76 
77 // This is required to avoid other libs (like PCL) to #define their own macros
78 // after including this header
79 #define DEG2RAD DEG2RAD
80 #define RAD2DEG RAD2DEG
81 
82 /** degrees to radian literal operator (e.g. `x=90.0_deg;`) */
83 constexpr inline double operator"" _deg(long double v)
84 {
85  return static_cast<double>(mrpt::DEG2RAD(v));
86 }
87 
88 /** Returns the sign of X as "1" or "-1" */
89 template <typename T>
90 inline int sign(T x)
91 {
92  return x < 0 ? -1 : 1;
93 }
94 
95 /** Returns the sign of X as "0", "1" or "-1" */
96 template <typename T>
97 inline int signWithZero(T x)
98 {
99  return (x == 0 || x == -0) ? 0 : sign(x);
100 }
101 
102 /** Returns the smallest positive number among a and b */
103 template <typename T>
104 T lowestPositive(const T a, const T b)
105 {
106  if (a > 0 && a <= b)
107  return a; // a positive and smaller than b
108  else if (b > 0)
109  return b; // b is positive and either smaller than a or a is negative
110  else
111  return a; // at least b is negative, we might not have an answer
112 }
113 
114 /** Efficient and portable evaluation of the absolute difference of two unsigned
115  * integer values
116  * (but will also work for signed and floating point types) */
117 template <typename T>
118 inline T abs_diff(const T a, const T b)
119 {
120  return std::max(a, b) - std::min(a, b);
121 }
122 
123 template <typename T>
124 inline const T min3(const T& A, const T& B, const T& C)
125 {
126  return std::min<T>(A, std::min<T>(B, C));
127 }
128 template <typename T>
129 inline const T max3(const T& A, const T& B, const T& C)
130 {
131  return std::max<T>(A, std::max<T>(B, C));
132 }
133 
134 /** Rounds toward zero */
135 template <typename T>
136 inline int fix(T x)
137 {
138  return x > 0 ? static_cast<int>(floor(static_cast<double>(x)))
139  : static_cast<int>(ceil(static_cast<double>(x)));
140 }
141 
142 /** If the second argument is below the first one, set the first argument to
143  * this lower value. */
144 template <typename T, typename K>
145 inline void keep_min(T& var, const K test_val)
146 {
147  if (test_val < var) var = static_cast<T>(test_val);
148 }
149 /** If the second argument is above the first one, set the first argument to
150  * this higher value. */
151 template <typename T, typename K>
152 inline void keep_max(T& var, const K test_val)
153 {
154  if (test_val > var) var = static_cast<T>(test_val);
155 }
156 /** Saturate the value of var (the variable gets modified) so it does not get
157  * out of [min,max]. */
158 template <typename T>
159 inline void saturate(T& var, const T sat_min, const T sat_max)
160 {
161  if (var > sat_max) var = sat_max;
162  if (var < sat_min) var = sat_min;
163 }
164 /** Like saturate() but it returns the value instead of modifying the variable
165  */
166 template <typename T>
167 inline T saturate_val(const T& value, const T sat_min, const T sat_max)
168 {
169  T var = value;
170  if (var > sat_max) var = sat_max;
171  if (var < sat_min) var = sat_min;
172  return var;
173 }
174 
175 /** Round up to the nearest power of two of a given number */
176 template <class T>
178 {
179  T n = 1;
180  while (n < val)
181  {
182  n <<= 1;
183  if (n <= 1) throw std::invalid_argument("round2up: Overflow!");
184  }
185  return n;
186 }
187 
188 /** shortcut for static_cast<float>(double) */
189 inline float d2f(const double d) { return static_cast<float>(d); }
190 
191 /** converts a float [0,1] into an uint8_t [0,255] (without checking for out of
192  * bounds) \sa u8tof */
193 inline uint8_t f2u8(const float f) { return static_cast<uint8_t>(f * 255); }
194 
195 /** converts a uint8_t [0,255] into a float [0,1] \sa f2u8 */
196 inline float u8tof(const uint8_t v) { return v / 255.0f; }
197 
198 /** @} */
199 
200 } // namespace mrpt
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
int fix(T x)
Rounds toward zero.
const T min3(const T &A, const T &B, const T &C)
void saturate(T &var, const T sat_min, const T sat_max)
Saturate the value of var (the variable gets modified) so it does not get out of [min,max].
float d2f(const double d)
shortcut for static_cast<float>(double)
constexpr double DEG2RAD(const double x)
Degrees to radians.
int val
Definition: mrpt_jpeglib.h:957
int sign(T x)
Returns the sign of X as "1" or "-1".
T round2up(T val)
Round up to the nearest power of two of a given number.
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
T lowestPositive(const T a, const T b)
Returns the smallest positive number among a and b.
return_t square(const num_t x)
Inline function for the square of a number.
uint8_t f2u8(const float f)
converts a float [0,1] into an uint8_t [0,255] (without checking for out of bounds) ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
constexpr double RAD2DEG(const double x)
Radians to degrees.
float u8tof(const uint8_t v)
converts a uint8_t [0,255] into a float [0,1]
T saturate_val(const T &value, const T sat_min, const T sat_max)
Like saturate() but it returns the value instead of modifying the variable.
const T max3(const T &A, const T &B, const T &C)
T abs_diff(const T a, const T b)
Efficient and portable evaluation of the absolute difference of two unsigned integer values (but will...
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020