MRPT  1.9.9
wrap2pi.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <cmath>
12 #include <cstddef> // size_t
13 
14 namespace mrpt::math
15 {
16 /** \addtogroup container_ops_grp
17  * @{ */
18 
19 /** Modifies the given angle to translate it into the [0,2pi[ range.
20  * \note Take care of not instancing this template for integer numbers, since
21  * it only works for float, double and long double.
22  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
23  */
24 template <class T>
25 inline void wrapTo2PiInPlace(T& a)
26 {
27  bool was_neg = a < 0;
28  a = static_cast<T>(fmod(a, static_cast<T>(2.0 * M_PI)));
29  if (was_neg) a += static_cast<T>(2.0 * M_PI);
30 }
31 
32 /** Modifies the given angle to translate it into the [0,2pi[ range.
33  * \note Take care of not instancing this template for integer numbers, since
34  * it only works for float, double and long double.
35  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
36  */
37 template <class T>
38 inline T wrapTo2Pi(T a)
39 {
41  return a;
42 }
43 
44 /** Modifies the given angle to translate it into the ]-pi,pi] range.
45  * \note Take care of not instancing this template for integer numbers, since
46  * it only works for float, double and long double.
47  * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
48  */
49 template <class T>
50 inline T wrapToPi(T a)
51 {
52  return wrapTo2Pi(a + static_cast<T>(M_PI)) - static_cast<T>(M_PI);
53 }
54 
55 /** Modifies the given angle to translate it into the ]-pi,pi] range.
56  * \note Take care of not instancing this template for integer numbers, since
57  * 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
67  * larger than PI in absolute value.
68  * \sa wrapToPi
69  */
70 template <class VECTOR>
71 void unwrap2PiSequence(VECTOR& x)
72 {
73  const size_t N = x.size();
74  for (size_t i = 0; i < N; i++)
75  {
76  mrpt::math::wrapToPiInPlace(x[i]); // assure it's in the -pi,pi range.
77  if (!i) continue;
78  double Ap = x[i] - x[i - 1];
79  if (Ap > M_PI) x[i] -= 2. * M_PI;
80  if (Ap < -M_PI) x[i] += 2. * M_PI;
81  }
82 }
83 
84 /** Computes the shortest angular increment (or distance) between two planar
85  * orientations,
86  * such that it is constrained to [-pi,pi] and is correct for any combination
87  * of angles (e.g. near +-pi)
88  * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
89  * angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
90  * \note Take care of not instancing this template for integer numbers, since
91  * it only works for float, double and long double.
92  * \sa wrapToPi, wrapTo2Pi
93  */
94 template <class T>
95 inline T angDistance(T from, T to)
96 {
97  wrapToPiInPlace(from);
98  wrapToPiInPlace(to);
99  T d = to - from;
100  if (d > M_PI)
101  d -= 2. * M_PI;
102  else if (d < -M_PI)
103  d += 2. * M_PI;
104  return d;
105 }
106 
107 /** @} */
108 
109 } // namespace mrpt::math
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:95
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
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:38
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
void wrapTo2PiInPlace(T &a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:25
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:71



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020