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) deltaPhase += M_TWOPI;

What are my options?

Answer 1

One-liner constant-time solution:

Okay, it's a two-liner if you count the second function for [min,max) form, but close enough — you could merge them together anyways.

/* change to `float/fmodf` or `long double/fmodl` or `int/%` as appropriate */

/* wrap x -> [0,max) */
double wrapMax(double x, double max)
{
    /* integer math: `(max + x % max) % max` */
    return fmod(max + fmod(x, max), max);
}
/* wrap x -> [min,max) */
double wrapMinMax(double x, double min, double max)
{
    return min + wrapMax(x - min, max - min);
}

Then you can simply use deltaPhase = wrapMinMax(deltaPhase, -M_PI, +M_PI).

The solutions is constant-time, meaning that the time it takes does not depend on how far your value is from [-PI,+PI) — for better or for worse.

Verification:

Now, I don't expect you to take my word for it, so here are some examples, including boundary conditions. I'm using integers for clarity, but it works much the same with fmod() and floats:

• Positive x:
  • wrapMax(3, 5) == 3: (5 + 3 % 5) % 5 == (5 + 3) % 5 == 8 % 5 == 3
  • wrapMax(6, 5) == 1: (5 + 6 % 5) % 5 == (5 + 1) % 5 == 6 % 5 == 1
• Negative x:
  • Note: These assume that integer modulo copies left-hand sign; if not, you get the above ("Positive") case.
  • wrapMax(-3, 5) == 2: (5 + (-3) % 5) % 5 == (5 - 3) % 5 == 2 % 5 == 2
  • wrapMax(-6, 5) == 4: (5 + (-6) % 5) % 5 == (5 - 1) % 5 == 4 % 5 == 4
• Boundaries:
  • wrapMax(0, 5) == 0: (5 + 0 % 5) % 5 == (5 + 0) % 5 == 5 % 5 == 0
  • wrapMax(5, 5) == 0: (5 + 5 % 5) % 5 == (5 + 0) % 5== 5 % 5 == 0
  • wrapMax(-5, 5) == 0: (5 + (-5) % 5) % 5 == (5 + 0) % 5 == 5 % 5 == 0
    • Note: Possibly -0 instead of +0 for floating-point.

The wrapMinMax function works much the same: wrapping x to [min,max) is the same as wrapping x - min to [0,max-min), and then (re-)adding min to the result.

I don't know what would happen with a negative max, but feel free to check that yourself!

Answer 2

Edit Apr 19, 2013:

Modulo function updated to handle boundary cases as noted by aka.nice and arr_sea:

static const double     _PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348;
static const double _TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696;

// Floating-point modulo
// The result (the remainder) has same sign as the divisor.
// Similar to matlab's mod(); Not similar to fmod() -   Mod(-3,4)= 1   fmod(-3,4)= -3
template<typename T>
T Mod(T x, T y)
{
    static_assert(!std::numeric_limits<T>::is_exact , "Mod: floating-point type expected");

    if (0. == y)
        return x;

    double m= x - y * floor(x/y);

    // handle boundary cases resulted from floating-point cut off:

    if (y > 0)              // modulo range: [0..y)
    {
        if (m>=y)           // Mod(-1e-16             , 360.    ): m= 360.
            return 0;

        if (m<0 )
        {
            if (y+m == y)
                return 0  ; // just in case...
            else
                return y+m; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14 
        }
    }
    else                    // modulo range: (y..0]
    {
        if (m<=y)           // Mod(1e-16              , -360.   ): m= -360.
            return 0;

        if (m>0 )
        {
            if (y+m == y)
                return 0  ; // just in case...
            else
                return y+m; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14 
        }
    }

    return m;
}

// wrap [rad] angle to [-PI..PI)
inline double WrapPosNegPI(double fAng)
{
    return Mod(fAng + _PI, _TWO_PI) - _PI;
}

// wrap [rad] angle to [0..TWO_PI)
inline double WrapTwoPI(double fAng)
{
    return Mod(fAng, _TWO_PI);
}

// wrap [deg] angle to [-180..180)
inline double WrapPosNeg180(double fAng)
{
    return Mod(fAng + 180., 360.) - 180.;
}

// wrap [deg] angle to [0..360)
inline double Wrap360(double fAng)
{
    return Mod(fAng ,360.);
}

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

If linking against glibc's libm (including newlib's implementation) you can access __ieee754_rem_pio2f() and __ieee754_rem_pio2() private functions:

extern __int32_t __ieee754_rem_pio2f (float,float*);

float wrapToPI(float xf){
const float p[4]={0,M_PI_2,M_PI,-M_PI_2};

    float yf[2];
    int q;
    int qmod4;

    q=__ieee754_rem_pio2f(xf,yf);

/* xf = q * M_PI_2 + yf[0] + yf[1]                 /
 * yf[1] << y[0], not sure if it could be ignored */

    qmod4= q % 4;

    if (qmod4==2) 
      /* (yf[0] > 0) defines interval (-pi,pi]*/
      return ( (yf[0] > 0) ?  -p[2] : p[2] ) + yf[0] + yf[1];
    else
      return p[qmod4] + yf[0] + yf[1];
}

Edit: Just realised that you need to link to libm.a, I couldn't find the symbols declared in libm.so

Answer 5

There is also fmod function in math.h but the sign causes trouble so that a subsequent operation is needed to make the result fir in the proper range (like you already do with the while's). For big values of deltaPhase this is probably faster than substracting/adding `M_TWOPI' hundreds of times.

deltaPhase = fmod(deltaPhase, M_TWOPI);

EDIT: I didn't try it intensively but I think you can use fmod this way by handling positive and negative values differently:

    if (deltaPhase>0)
        deltaPhase = fmod(deltaPhase+M_PI, 2.0*M_PI)-M_PI;
    else
        deltaPhase = fmod(deltaPhase-M_PI, 2.0*M_PI)+M_PI;

The computational time is constant (unlike the while solution which gets slower as the absolute value of deltaPhase increases)

Answer 6

A two-liner, non-iterative, tested solution for normalizing arbitrary angles to [-π, π):

double normalizeAngle(double angle)
{
    double a = fmod(angle + M_PI, 2 * M_PI);
    return a >= 0 ? (a - M_PI) : (a + M_PI);
}

Similarly, for [0, 2π):

double normalizeAngle(double angle)
{
    double a = fmod(angle, 2 * M_PI);
    return a >= 0 ? a : (a + 2 * M_PI);
}