Main MRPT website > C++ reference for 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 |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
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
16 {
17 namespace math
18 {
19 /** \addtogroup container_ops_grp
20  * @{ */
21 
22 /** Modifies the given angle to translate it into the [0,2pi[ range.
23  * \note Take care of not instancing this template for integer numbers, since
24  * it only works for float, double and long double.
25  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
26  */
27 template <class T>
28 inline void wrapTo2PiInPlace(T& a)
29 {
30  bool was_neg = a < 0;
31  a = fmod(a, static_cast<T>(2.0 * M_PI));
32  if (was_neg) a += static_cast<T>(2.0 * M_PI);
33 }
34 
35 /** Modifies the given angle to translate it into the [0,2pi[ range.
36  * \note Take care of not instancing this template for integer numbers, since
37  * it only works for float, double and long double.
38  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
39  */
40 template <class T>
41 inline T wrapTo2Pi(T a)
42 {
44  return a;
45 }
46 
47 /** Modifies the given angle to translate it into the ]-pi,pi] range.
48  * \note Take care of not instancing this template for integer numbers, since
49  * it only works for float, double and long double.
50  * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
51  */
52 template <class T>
53 inline T wrapToPi(T a)
54 {
55  return wrapTo2Pi(a + static_cast<T>(M_PI)) - static_cast<T>(M_PI);
56 }
57 
58 /** Modifies the given angle to translate it into the ]-pi,pi] range.
59  * \note Take care of not instancing this template for integer numbers, since
60  * it only works for float, double and long double.
61  * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
62  */
63 template <class T>
64 inline void wrapToPiInPlace(T& a)
65 {
66  a = wrapToPi(a);
67 }
68 
69 /** Modify a sequence of angle values such as no consecutive values have a jump
70  * larger than PI in absolute value.
71  * \sa wrapToPi
72  */
73 template <class VECTOR>
74 void unwrap2PiSequence(VECTOR& x)
75 {
76  const size_t N = x.size();
77  for (size_t i = 0; i < N; i++)
78  {
79  mrpt::math::wrapToPiInPlace(x[i]); // assure it's in the -pi,pi range.
80  if (!i) continue;
81  double Ap = x[i] - x[i - 1];
82  if (Ap > M_PI) x[i] -= 2. * M_PI;
83  if (Ap < -M_PI) x[i] += 2. * M_PI;
84  }
85 }
86 
87 /** Computes the shortest angular increment (or distance) between two planar
88  * orientations,
89  * such that it is constrained to [-pi,pi] and is correct for any combination
90  * of angles (e.g. near +-pi)
91  * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
92  * angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
93  * \note Take care of not instancing this template for integer numbers, since
94  * it only works for float, double and long double.
95  * \sa wrapToPi, wrapTo2Pi
96  */
97 template <class T>
98 inline T angDistance(T from, T to)
99 {
100  wrapToPiInPlace(from);
101  wrapToPiInPlace(to);
102  T d = to - from;
103  if (d > M_PI)
104  d -= 2. * M_PI;
105  else if (d < -M_PI)
106  d += 2. * M_PI;
107  return d;
108 }
109 
110 /** @} */
111 
112 } // End of MATH namespace
113 } // End of namespace
114 
115 #endif
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:98
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:64
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:41
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
void wrapTo2PiInPlace(T &a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:74
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019