wrap2pi.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <cmath>
#include <cstddef>  // size_t

namespace mrpt::math
{
/** \addtogroup container_ops_grp
 * @{ */

/** Modifies the given angle to translate it into the [0,2pi[ range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
 */
template <class T>
inline void wrapTo2PiInPlace(T& a)
{
    bool was_neg = a < 0;
    a = static_cast<T>(fmod(a, static_cast<T>(2.0 * M_PI)));
    if (was_neg) a += static_cast<T>(2.0 * M_PI);
}

/** Modifies the given angle to translate it into the [0,2pi[ range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
 */
template <class T>
inline T wrapTo2Pi(T a)
{
    wrapTo2PiInPlace(a);
    return a;
}

/** Modifies the given angle to translate it into the ]-pi,pi] range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
 */
template <class T>
inline T wrapToPi(T a)
{
    return wrapTo2Pi(a + static_cast<T>(M_PI)) - static_cast<T>(M_PI);
}

/** Modifies the given angle to translate it into the ]-pi,pi] range.
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
 */
template <class T>
inline void wrapToPiInPlace(T& a)
{
    a = wrapToPi(a);
}

/** Modify a sequence of angle values such as no consecutive values have a jump
 * larger than PI in absolute value.
 * \sa wrapToPi
 */
template <class VECTOR>
void unwrap2PiSequence(VECTOR& x)
{
    const size_t N = x.size();
    for (size_t i = 0; i < N; i++)
    {
        mrpt::math::wrapToPiInPlace(x[i]);  // assure it's in the -pi,pi range.
        if (!i) continue;
        double Ap = x[i] - x[i - 1];
        if (Ap > M_PI) x[i] -= 2. * M_PI;
        if (Ap < -M_PI) x[i] += 2. * M_PI;
    }
}

/** 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. near +-pi)
 * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
 *           angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
 * \note Take care of not instancing this template for integer numbers, since
 * it only works for float, double and long double.
 * \sa wrapToPi, wrapTo2Pi
 */
template <class T>
inline T angDistance(T from, T to)
{
    wrapToPiInPlace(from);
    wrapToPiInPlace(to);
    T d = to - from;
    if (d > M_PI)
        d -= 2. * M_PI;
    else if (d < -M_PI)
        d += 2. * M_PI;
    return d;
}

/** @} */

}  // namespace mrpt::math