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 /** Inline function for the square of a number. */
18 template <class T>
19 inline T square(const T x)
20 {
21  return x * x;
22 }
23 
24 /** Faster version of std::hypot(), to use when overflow is not an issue and we
25  * prefer fast code. */
26 template <class T>
27 inline T hypot_fast(const T x, const T y)
28 {
29  return std::sqrt(x * x + y * y);
30 }
31 
32 #ifdef DEG2RAD // functions are preferred over macros
33 #undef DEG2RAD
34 #endif
35 #ifdef RAD2DEG
36 #undef RAD2DEG
37 #endif
38 #if !defined(M_PI)
39 #define M_PI 3.14159265358979323846
40 #endif
41 
42 /** Degrees to radians */
43 inline double DEG2RAD(const double x) { return x * M_PI / 180.0; }
44 /** Degrees to radians */
45 inline float DEG2RAD(const float x) { return x * float(M_PI) / 180.0f; }
46 /** Degrees to radians */
47 inline double DEG2RAD(const int x) { return x * M_PI / 180.0; }
48 /** Radians to degrees */
49 inline double RAD2DEG(const double x) { return x * 180.0 / M_PI; }
50 /** Radians to degrees */
51 inline float RAD2DEG(const float x) { return x * 180.0f / float(M_PI); }
52 #if !defined(M_PIl)
53 #define M_PIl 3.14159265358979323846264338327950288L
54 #define M_2PIl (2.0L * 3.14159265358979323846264338327950288L)
55 #endif
56 /** Degrees to radians */
57 inline long double DEG2RAD(const long double x) { return x * M_PIl / 180.0; }
58 /** Radians to degrees */
59 inline long double RAD2DEG(const long double x) { return x * 180.0 / M_PIl; }
60 
61 // This is required to avoid other libs (like PCL) to #define their own macros
62 // after including this header
63 #define DEG2RAD DEG2RAD
64 #define RAD2DEG RAD2DEG
65 
66 /** Returns the sign of X as "1" or "-1" */
67 template <typename T>
68 inline int sign(T x)
69 {
70  return x < 0 ? -1 : 1;
71 }
72 
73 /** Returns the sign of X as "0", "1" or "-1" */
74 template <typename T>
75 inline int signWithZero(T x)
76 {
77  return (x == 0 || x == -0) ? 0 : sign(x);
78 }
79 
80 /** Returns the lowest, possitive among two numbers. If both are non-positive
81  * (<=0), the lowest one is returned. */
82 template <typename T>
83 T lowestPositive(const T a, const T b)
84 {
85  if (a > 0 && a <= b)
86  return a; // a positive and smaller than b
87  else if (b > 0)
88  return b; // b is positive and either smaller than a or a is negative
89  else
90  return a; // at least b is negative, we might not have an answer
91 }
92 
93 /** Efficient and portable evaluation of the absolute difference of two unsigned
94  * integer values
95  * (but will also work for signed and floating point types) */
96 template <typename T>
97 inline T abs_diff(const T a, const T b)
98 {
99  return std::max(a, b) - std::min(a, b);
100 }
101 
102 template <typename T>
103 inline const T min3(const T& A, const T& B, const T& C)
104 {
105  return std::min<T>(A, std::min<T>(B, C));
106 }
107 template <typename T>
108 inline const T max3(const T& A, const T& B, const T& C)
109 {
110  return std::max<T>(A, std::max<T>(B, C));
111 }
112 
113 /** Rounds toward zero */
114 template <typename T>
115 inline int fix(T x)
116 {
117  return x > 0 ? static_cast<int>(floor(static_cast<double>(x)))
118  : static_cast<int>(ceil(static_cast<double>(x)));
119 }
120 
121 /** If the second argument is below the first one, set the first argument to
122  * this lower value. */
123 template <typename T, typename K>
124 inline void keep_min(T& var, const K test_val)
125 {
126  if (test_val < var) var = test_val;
127 }
128 /** If the second argument is above the first one, set the first argument to
129  * this higher value. */
130 template <typename T, typename K>
131 inline void keep_max(T& var, const K test_val)
132 {
133  if (test_val > var) var = test_val;
134 }
135 /** Saturate the value of var (the variable gets modified) so it does not get
136  * out of [min,max]. */
137 template <typename T>
138 inline void saturate(T& var, const T sat_min, const T sat_max)
139 {
140  if (var > sat_max) var = sat_max;
141  if (var < sat_min) var = sat_min;
142 }
143 /** Like saturate() but it returns the value instead of modifying the variable
144  */
145 template <typename T>
146 inline T saturate_val(const T& value, const T sat_min, const T sat_max)
147 {
148  T var = value;
149  if (var > sat_max) var = sat_max;
150  if (var < sat_min) var = sat_min;
151  return var;
152 }
153 
154 /** Round up to the nearest power of two of a given number */
155 template <class T>
157 {
158  T n = 1;
159  while (n < val)
160  {
161  n <<= 1;
162  if (n <= 1) throw std::invalid_argument("round2up: Overflow!");
163  }
164  return n;
165 }
166 } // namespace mrpt
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].
#define min(a, b)
double RAD2DEG(const double x)
Radians to degrees.
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...
T lowestPositive(const T a, const T b)
Returns the lowest, possitive among two numbers.
int fix(T x)
Rounds toward zero.
double DEG2RAD(const double x)
Degrees to radians.
const T max3(const T &A, const T &B, const T &C)
GLenum GLsizei n
Definition: glext.h:5136
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
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...
T square(const T x)
Inline function for the square of a number.
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 abs_diff(const T a, const T b)
Efficient and portable evaluation of the absolute difference of two unsigned integer values (but will...
T round2up(T val)
Round up to the nearest power of two of a given number.
int val
Definition: mrpt_jpeglib.h:957
GLubyte GLubyte b
Definition: glext.h:6372
const T min3(const T &A, const T &B, const T &C)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLenum GLint GLint y
Definition: glext.h:3542
int sign(T x)
Returns the sign of X as "1" or "-1".
GLsizei const GLfloat * value
Definition: glext.h:4134
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.
GLenum GLint x
Definition: glext.h:3542
GLubyte GLubyte GLubyte a
Definition: glext.h:6372



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019