Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 #define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath)
13 #include <cmath>
14 #include <cstddef> // size_t
15 
16 namespace mrpt
17 {
18  namespace math
19  {
20  /** \addtogroup container_ops_grp
21  * @{ */
22 
23  /** Modifies the given angle to translate it into the [0,2pi[ range.
24  * \note Take care of not instancing this template for integer numbers, since 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 it only works for float, double and long double.
37  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
38  */
39  template <class T>
40  inline T wrapTo2Pi(T a)
41  {
43  return a;
44  }
45 
46  /** Modifies the given angle to translate it into the ]-pi,pi] range.
47  * \note Take care of not instancing this template for integer numbers, since 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 it only works for float, double and long double.
58  * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
59  */
60  template <class T>
61  inline void wrapToPiInPlace(T &a)
62  {
63  a = wrapToPi(a);
64  }
65 
66  /** Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolute value.
67  * \sa wrapToPi
68  */
69  template <class VECTOR>
70  void unwrap2PiSequence(VECTOR &x)
71  {
72  const size_t N=x.size();
73  for (size_t i=0;i<N;i++)
74  {
75  mrpt::math::wrapToPiInPlace(x[i]); // assure it's in the -pi,pi range.
76  if (!i) continue;
77  double Ap = x[i]-x[i-1];
78  if (Ap>M_PI) x[i]-=2.*M_PI;
79  if (Ap<-M_PI) x[i]+=2.*M_PI;
80  }
81  }
82 
83  /** Computes the shortest angular increment (or distance) between two planar orientations,
84  * such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g. near +-pi)
85  * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
86  * angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
87  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
88  * \sa wrapToPi, wrapTo2Pi
89  */
90  template <class T>
91  inline T angDistance(T from, T to)
92  {
93  wrapToPiInPlace(from);
94  wrapToPiInPlace(to);
95  T d = to-from;
96  if (d>M_PI) d-=2.*M_PI;
97  else if (d<-M_PI) d+=2.*M_PI;
98  return d;
99  }
100 
101  /** @} */
102 
103  } // End of MATH namespace
104 } // End of namespace
105 
106 #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:91
#define M_PI
Definition: bits.h:78
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:40
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: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:70
GLenum GLint x
Definition: glext.h:3516
GLubyte GLubyte GLubyte a
Definition: glext.h:5575



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019