MRPT  1.9.9
wrap2pi.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9 #ifndef MRPT_MATH_WRAP2PI_H
10 #define MRPT_MATH_WRAP2PI_H
11
12 #include <cmath>
13 #include <cstddef> // size_t
14
15 namespace mrpt::math
16 {
18  * @{ */
19
20 /** Modifies the given angle to translate it into the [0,2pi[ range.
21  * \note Take care of not instancing this template for integer numbers, since
22  * it only works for float, double and long double.
23  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
24  */
25 template <class T>
26 inline void wrapTo2PiInPlace(T& a)
27 {
28  bool was_neg = a < 0;
29  a = fmod(a, static_cast<T>(2.0 * M_PI));
30  if (was_neg) a += static_cast<T>(2.0 * M_PI);
31 }
32
33 /** Modifies the given angle to translate it into the [0,2pi[ range.
34  * \note Take care of not instancing this template for integer numbers, since
35  * it only works for float, double and long double.
36  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
37  */
38 template <class T>
39 inline T wrapTo2Pi(T a)
40 {
42  return a;
43 }
44
45 /** Modifies the given angle to translate it into the ]-pi,pi] range.
46  * \note Take care of not instancing this template for integer numbers, since
47  * it only works for float, double and long double.
48  * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
49  */
50 template <class T>
51 inline T wrapToPi(T a)
52 {
53  return wrapTo2Pi(a + static_cast<T>(M_PI)) - static_cast<T>(M_PI);
54 }
55
56 /** Modifies the given angle to translate it into the ]-pi,pi] range.
57  * \note Take care of not instancing this template for integer numbers, since
58  * it only works for float, double and long double.
59  * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
60  */
61 template <class T>
62 inline void wrapToPiInPlace(T& a)
63 {
64  a = wrapToPi(a);
65 }
66
67 /** Modify a sequence of angle values such as no consecutive values have a jump
68  * larger than PI in absolute value.
69  * \sa wrapToPi
70  */
71 template <class VECTOR>
72 void unwrap2PiSequence(VECTOR& x)
73 {
74  const size_t N = x.size();
75  for (size_t i = 0; i < N; i++)
76  {
77  mrpt::math::wrapToPiInPlace(x[i]); // assure it's in the -pi,pi range.
78  if (!i) continue;
79  double Ap = x[i] - x[i - 1];
80  if (Ap > M_PI) x[i] -= 2. * M_PI;
81  if (Ap < -M_PI) x[i] += 2. * M_PI;
82  }
83 }
84
85 /** Computes the shortest angular increment (or distance) between two planar
86  * orientations,
87  * such that it is constrained to [-pi,pi] and is correct for any combination
88  * of angles (e.g. near +-pi)
89  * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
90  * angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
91  * \note Take care of not instancing this template for integer numbers, since
92  * it only works for float, double and long double.
93  * \sa wrapToPi, wrapTo2Pi
94  */
95 template <class T>
96 inline T angDistance(T from, T to)
97 {
98  wrapToPiInPlace(from);
99  wrapToPiInPlace(to);
100  T d = to - from;
101  if (d > M_PI)
102  d -= 2. * M_PI;
103  else if (d < -M_PI)
104  d += 2. * M_PI;
105  return d;
106 }
107
108 /** @} */
109
110 }
111 #endif
112
113
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:96
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:62
This base provides a set of functions for maths stuff.
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:39
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
void wrapTo2PiInPlace(T &a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:26
void unwrap2PiSequence(VECTOR &x)
Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolut...
Definition: wrap2pi.h:72
GLenum GLint x
Definition: glext.h:3538
GLubyte GLubyte GLubyte a
Definition: glext.h:6279

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020