Math.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
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 #pragma once
19 
20 #ifndef __OpenKarto_Math_h__
21 #define __OpenKarto_Math_h__
22 
23 #include <assert.h>
24 #include <math.h>
25 #include <float.h>
26 #include <cstdlib>
27 
28 #include <OpenKarto/Types.h>
29 
30 namespace karto
31 {
32 
34 
35 
43  const kt_double KT_PI = 3.14159265358979323846;
44 
48  const kt_double KT_2PI = 6.28318530717958647692;
49 
53  const kt_double KT_PI_2 = 1.57079632679489661923;
54 
58  const kt_double KT_PI_180 = 0.01745329251994329577;
59 
63  const kt_double KT_180_PI = 57.29577951308232087685;
64 
71  const kt_double KT_TOLERANCE = 1e-06;
72 
73  namespace math
74  {
76 
77 
84  {
85  return degrees * KT_PI_180;
86  }
87 
94  {
95  return radians * KT_180_PI;
96  }
97 
103  template<typename T>
104  inline T Square(T value)
105  {
106  return (value * value);
107  }
108 
114  inline kt_double Round(kt_double value)
115  {
116  return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
117  }
118 
125  template<typename T>
126  inline const T& Minimum(const T& value1, const T& value2)
127  {
128  return value1 < value2 ? value1 : value2;
129  }
130 
137  template<typename T>
138  inline const T& Maximum(const T& value1, const T& value2)
139  {
140  return value1 > value2 ? value1 : value2;
141  }
142 
150  template<typename T>
151  inline const T& Clip(const T& n, const T& minValue, const T& maxValue)
152  {
153  return Minimum(Maximum(n, minValue), maxValue);
154  }
155 
163  {
164  double delta = a - b;
165  return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
166  }
167 
174  template<typename T>
175  inline kt_bool IsUpTo(const T& value, const T& maximum)
176  {
177  return (value >= 0 && value < maximum);
178  }
179 
180  //@cond EXCLUDE
188  template<>
189  inline kt_bool IsUpTo<kt_int32u>(const kt_int32u& value, const kt_int32u& maximum)
190  {
191  return (value < maximum);
192  }
193  //@endcond
194 
202  template<typename T>
203  inline kt_bool InRange(const T& value, const T& a, const T& b)
204  {
205  return (value >= a && value <= b);
206  }
207 
214  {
215  while (angle < -KT_PI)
216  {
217  if (angle < -KT_2PI)
218  {
219  angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI;
220  }
221  else
222  {
223  angle += KT_2PI;
224  }
225  }
226 
227  while (angle > KT_PI)
228  {
229  if (angle > KT_2PI)
230  {
231  angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI;
232  }
233  else
234  {
235  angle -= KT_2PI;
236  }
237  }
238 
239  assert(math::InRange(angle, -KT_PI, KT_PI));
240 
241  return angle;
242  }
243 
253  {
254  while (minuend - subtrahend < -KT_PI)
255  {
256  minuend += KT_2PI;
257  }
258 
259  while (minuend - subtrahend > KT_PI)
260  {
261  minuend -= KT_2PI;
262  }
263 
264  return minuend;
265  }
266 
274  template<class T>
275  inline T AlignValue(size_t value, size_t alignValue = 8)
276  {
277  return static_cast<T> ((value + (alignValue - 1)) & ~(alignValue - 1));
278  }
279 
285  template<class T>
286  inline void Swap(T& x, T& y)
287  {
288  T temp = x;
289  x = y;
290  y = temp;
291  }
292 
294  } // Math
295 
297 }
298 
299 #endif // __OpenKarto_Math_h__
const kt_double KT_2PI
Definition: Math.h:48
bool kt_bool
Definition: Types.h:145
T Square(T value)
Definition: Math.h:104
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:138
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:175
kt_double Round(kt_double value)
Definition: Math.h:114
const kt_double KT_PI_2
Definition: Math.h:53
uint32_t kt_int32u
Definition: Types.h:111
const kt_double KT_PI_180
Definition: Math.h:58
const kt_double KT_TOLERANCE
Definition: Math.h:71
const T & Minimum(const T &value1, const T &value2)
Definition: Math.h:126
T AlignValue(size_t value, size_t alignValue=8)
Definition: Math.h:275
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:151
const kt_double KT_180_PI
Definition: Math.h:63
double kt_double
Definition: Types.h:160
kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
Definition: Math.h:252
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:83
const kt_double KT_PI
Definition: Math.h:43
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:213
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:203
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:162
void Swap(T &x, T &y)
Definition: Math.h:286
Definition: Any.cpp:20
kt_double RadiansToDegrees(kt_double radians)
Definition: Math.h:93


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24