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-2019, 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 <class T>
23 inline T square(const T x)
24 {
25  return 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 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 = 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 = 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 /** @} */
189 
190 } // 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.
GLenum GLsizei n
Definition: glext.h:5136
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].
T square(const T x)
Inline function for the square of a number.
constexpr double DEG2RAD(const double x)
Degrees to radians.
int val
Definition: mrpt_jpeglib.h:957
GLubyte GLubyte b
Definition: glext.h:6372
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.
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
constexpr double RAD2DEG(const double x)
Radians to degrees.
GLenum GLint GLint y
Definition: glext.h:3542
GLsizei const GLfloat * value
Definition: glext.h:4134
GLenum GLint x
Definition: glext.h:3542
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.
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
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: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019