C: How to wrap a float to the interval [-pi, pi)

Question

I\'m looking for some nice C code that will accomplish effectively:

while (deltaPhase >= M_PI) deltaPhase -= M_TWOPI;
while (deltaPhase < -M_PI) deltaP         


        

Answer 1

I would do this:

double wrap(double x) {
    return x-2*M_PI*floor(x/(2*M_PI)+0.5);  
}

There will be significant numerical errors. The best solution to the numerical errors is to store your phase scaled by 1/PI or by 1/(2*PI) and depending on what you are doing store them as fixed point.

Answer 2

Here is a version for other people finding this question that can use C++ with Boost:

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sign.hpp>

template<typename T>
inline T normalizeRadiansPiToMinusPi(T rad)
{
  // copy the sign of the value in radians to the value of pi
  T signedPI = boost::math::copysign(boost::math::constants::pi<T>(),rad);
  // set the value of rad to the appropriate signed value between pi and -pi
  rad = fmod(rad+signedPI,(2*boost::math::constants::pi<T>())) - signedPI;

  return rad;
} 

C++11 version, no Boost dependency:

#include <cmath>

// Bring the 'difference' between two angles into [-pi; pi].
template <typename T>
T normalizeRadiansPiToMinusPi(T rad) {
  // Copy the sign of the value in radians to the value of pi.
  T signed_pi = std::copysign(M_PI,rad);
  // Set the value of difference to the appropriate signed value between pi and -pi.
  rad = std::fmod(rad + signed_pi,(2 * M_PI)) - signed_pi;
  return rad;
}

Answer 3

In the case where fmod() is implemented through truncated division and has the same sign as the dividend, it can be taken advantage of to solve the general problem thusly:

For the case of (-PI, PI]:

if (x > 0) x = x - 2PI * ceil(x/2PI)  #Shift to the negative regime
return fmod(x - PI, 2PI) + PI

And for the case of [-PI, PI):

if (x < 0) x = x - 2PI * floor(x/2PI)  #Shift to the positive regime
return fmod(x + PI, 2PI) - PI

[Note that this is pseudocode; my original was written in Tcl, and I didn't want to torture everyone with that. I needed the first case, so had to figure this out.]

Answer 4

The way suggested you suggested is best. It is fastest for small deflections. If angles in your program are constantly being deflected into the proper range, then you should only run into big out of range values rarely. Therefore paying the cost of a complicated modular arithmetic code every round seems wasteful. Comparisons are cheap compared to modular arithmetic (http://embeddedgurus.com/stack-overflow/2011/02/efficient-c-tip-13-use-the-modulus-operator-with-caution/).

Answer 5

If ever your input angle can reach arbitrarily high values, and if continuity matters, you can also try

atan2(sin(x),cos(x))

This will preserve continuity of sin(x) and cos(x) better than modulo for high values of x, especially in single precision (float).

Indeed, exact_value_of_pi - double_precision_approximation ~= 1.22e-16

On the other hand, most library/hardware use a high precision approximation of PI for applying the modulo when evaluating trigonometric functions (though x86 family is known to use a rather poor one).

Result might be in [-pi,pi], you'll have to check the exact bounds.

Personaly, I would prevent any angle to reach several revolutions by wrapping systematically and stick to a fmod solution like the one of boost.

Answer 6

deltaPhase -= floor(deltaPhase/M_TWOPI)*M_TWOPI;