Math.h
Go to the documentation of this file.
1 /*
2  * Copyright 2010 SRI International
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #ifndef karto_sdk_MATH_H
19 #define karto_sdk_MATH_H
20 
21 #include <assert.h>
22 #include <math.h>
23 #include <limits>
24 
25 #include <karto_sdk/Types.h>
26 
27 namespace karto
28 {
32  const kt_double KT_PI = 3.14159265358979323846; // The value of PI
33  const kt_double KT_2PI = 6.28318530717958647692; // 2 * PI
34  const kt_double KT_PI_2 = 1.57079632679489661923; // PI / 2
35  const kt_double KT_PI_180 = 0.01745329251994329577; // PI / 180
36  const kt_double KT_180_PI = 57.29577951308232087685; // 180 / PI
37 
41  const kt_double KT_TOLERANCE = 1e-06;
42 
47  const kt_int32s INVALID_SCAN = std::numeric_limits<kt_int32s>::max();
48 
49  namespace math
50  {
57  {
58  return degrees * KT_PI_180;
59  }
60 
67  {
68  return radians * KT_180_PI;
69  }
70 
76  template<typename T>
77  inline T Square(T value)
78  {
79  return (value * value);
80  }
81 
87  inline kt_double Round(kt_double value)
88  {
89  return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
90  }
91 
98  template<typename T>
99  inline const T& Minimum(const T& value1, const T& value2)
100  {
101  return value1 < value2 ? value1 : value2;
102  }
103 
110  template<typename T>
111  inline const T& Maximum(const T& value1, const T& value2)
112  {
113  return value1 > value2 ? value1 : value2;
114  }
115 
123  template<typename T>
124  inline const T& Clip(const T& n, const T& minValue, const T& maxValue)
125  {
126  return Minimum(Maximum(n, minValue), maxValue);
127  }
128 
136  {
137  double delta = a - b;
138  return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
139  }
140 
146  template<typename T>
147  inline kt_bool IsUpTo(const T& value, const T& maximum)
148  {
149  return (value >= 0 && value < maximum);
150  }
151 
158  template<>
159  inline kt_bool IsUpTo<kt_int32u>(const kt_int32u& value, const kt_int32u& maximum)
160  {
161  return (value < maximum);
162  }
163 
164 
171  template<typename T>
172  inline kt_bool InRange(const T& value, const T& a, const T& b)
173  {
174  return (value >= a && value <= b);
175  }
176 
183  {
184  while (angle < -KT_PI)
185  {
186  if (angle < -KT_2PI)
187  {
188  angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI;
189  }
190  else
191  {
192  angle += KT_2PI;
193  }
194  }
195 
196  while (angle > KT_PI)
197  {
198  if (angle > KT_2PI)
199  {
200  angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI;
201  }
202  else
203  {
204  angle -= KT_2PI;
205  }
206  }
207 
208  assert(math::InRange(angle, -KT_PI, KT_PI));
209 
210  return angle;
211  }
212 
222  {
223  while (minuend - subtrahend < -KT_PI)
224  {
225  minuend += KT_2PI;
226  }
227 
228  while (minuend - subtrahend > KT_PI)
229  {
230  minuend -= KT_2PI;
231  }
232 
233  return minuend;
234  }
235 
243  template<class T>
244  inline T AlignValue(size_t value, size_t alignValue = 8)
245  {
246  return static_cast<T> ((value + (alignValue - 1)) & ~(alignValue - 1));
247  }
248  } // namespace math
249 
250 } // namespace karto
251 
252 #endif // karto_sdk_MATH_H
karto::math::Round
kt_double Round(kt_double value)
Definition: Math.h:87
karto::KT_180_PI
const kt_double KT_180_PI
Definition: Math.h:36
karto::KT_TOLERANCE
const kt_double KT_TOLERANCE
Definition: Math.h:41
karto::KT_PI
const kt_double KT_PI
Definition: Math.h:32
karto::math::Minimum
const T & Minimum(const T &value1, const T &value2)
Definition: Math.h:99
kt_double
double kt_double
Definition: Types.h:67
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Types.h
karto::math::DegreesToRadians
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:56
karto::KT_PI_180
const kt_double KT_PI_180
Definition: Math.h:35
kt_int32s
int32_t kt_int32s
Definition: Types.h:51
karto::math::Maximum
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:111
karto::math::IsUpTo< kt_int32u >
kt_bool IsUpTo< kt_int32u >(const kt_int32u &value, const kt_int32u &maximum)
Definition: Math.h:159
karto::math::AlignValue
T AlignValue(size_t value, size_t alignValue=8)
Definition: Math.h:244
karto::math::Clip
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:124
karto::math::RadiansToDegrees
kt_double RadiansToDegrees(kt_double radians)
Definition: Math.h:66
karto::math::NormalizeAngle
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:182
kt_bool
bool kt_bool
Definition: Types.h:64
karto::math::InRange
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
karto::KT_PI_2
const kt_double KT_PI_2
Definition: Math.h:34
karto::math::DoubleEqual
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:135
karto::KT_2PI
const kt_double KT_2PI
Definition: Math.h:33
karto::math::Square
T Square(T value)
Definition: Math.h:77
kt_int32u
uint32_t kt_int32u
Definition: Types.h:52
karto::math::NormalizeAngleDifference
kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
Definition: Math.h:221
karto::math::IsUpTo
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
assert.h
karto::INVALID_SCAN
const kt_int32s INVALID_SCAN
Definition: Math.h:47
karto
Definition: Karto.h:88


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55