Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
16namespace 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);
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
#define M_PI
Definition: bits.h:65
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
Definition: wrap2pi.h:91
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
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.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Feb 15 01:46:32 UTC 2023