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 have used (in python):

def WrapAngle(Wrapped, UnWrapped ):
    TWOPI = math.pi * 2
    TWOPIINV = 1.0 / TWOPI
    return  UnWrapped + round((Wrapped - UnWrapped) * TWOPIINV) * TWOPI

c-code equivalent:

#define TWOPI 6.28318531

double WrapAngle(const double dWrapped, const double dUnWrapped )
{   
    const double TWOPIINV = 1.0/ TWOPI;
    return  dUnWrapped + round((dWrapped - dUnWrapped) * TWOPIINV) * TWOPI;
}

notice that this brings it in the wrapped domain +/- 2pi so for +/- pi domain you need to handle that afterward like:

if( angle > pi):
    angle -= 2*math.pi

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

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!