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 #define DEG2RAD \
61  DEG2RAD // This is required to avoid other libs (like PCL) to #define their
62 // own versions of DEG2RAD
63 #define RAD2DEG \
64  RAD2DEG // This is required to avoid other libs (like PCL) to #define their
65 // own versions of RAD2DEG
66 
67 /** Returns the sign of X as "1" or "-1" */
68 template <typename T>
69 inline int sign(T x)
70 {
71  return x < 0 ? -1 : 1;
72 }
73 
74 /** Returns the sign of X as "0", "1" or "-1" */
75 template <typename T>
76 inline int signWithZero(T x)
77 {
78  return (x == 0 || x == -0) ? 0 : sign(x);
79 }
80 
81 /** Returns the lowest, possitive among two numbers. If both are non-positive
82  * (<=0), the lowest one is returned. */
83 template <typename T>
84 T lowestPositive(const T a, const T b)
85 {
86  if (a > 0 && a <= b)
87  return a; // a positive and smaller than b
88  else if (b > 0)
89  return b; // b is positive and either smaller than a or a is negative
90  else
91  return a; // at least b is negative, we might not have an answer
92 }
93 
94 /** Efficient and portable evaluation of the absolute difference of two unsigned
95  * integer values
96  * (but will also work for signed and floating point types) */
97 template <typename T>
98 inline T abs_diff(const T a, const T b)
99 {
100  return std::max(a, b) - std::min(a, b);
101 }
102 
103 template <typename T>
104 inline const T min3(const T& A, const T& B, const T& C)
105 {
106  return std::min<T>(A, std::min<T>(B, C));
107 }
108 template <typename T>
109 inline const T max3(const T& A, const T& B, const T& C)
110 {
111  return std::max<T>(A, std::max<T>(B, C));
112 }
113 
114 /** Rounds toward zero */
115 template <typename T>
116 inline int fix(T x)
117 {
118  return x > 0 ? static_cast<int>(floor(static_cast<double>(x)))
119  : static_cast<int>(ceil(static_cast<double>(x)));
120 }
121 
122 /** If the second argument is below the first one, set the first argument to
123  * this lower value. */
124 template <typename T, typename K>
125 inline void keep_min(T& var, const K test_val)
126 {
127  if (test_val < var) var = test_val;
128 }
129 /** If the second argument is above the first one, set the first argument to
130  * this higher value. */
131 template <typename T, typename K>
132 inline void keep_max(T& var, const K test_val)
133 {
134  if (test_val > var) var = test_val;
135 }
136 /** Saturate the value of var (the variable gets modified) so it does not get
137  * out of [min,max]. */
138 template <typename T>
139 inline void saturate(T& var, const T sat_min, const T sat_max)
140 {
141  if (var > sat_max) var = sat_max;
142  if (var < sat_min) var = sat_min;
143 }
144 /** Like saturate() but it returns the value instead of modifying the variable
145  */
146 template <typename T>
147 inline T saturate_val(const T& value, const T sat_min, const T sat_max)
148 {
149  T var = value;
150  if (var > sat_max) var = sat_max;
151  if (var < sat_min) var = sat_min;
152  return var;
153 }
154 
155 /** Round up to the nearest power of two of a given number */
156 template <class T>
158 {
159  T n = 1;
160  while (n < val)
161  {
162  n <<= 1;
163  if (n <= 1) throw std::invalid_argument("round2up: Overflow!");
164  }
165  return n;
166 }
167 } // 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: 6902d6456 Fri Jun 14 09:26:43 2019 +0200 at vie jun 14 09:30:16 CEST 2019