Dealing with Angle Wrap in c++ code

Is there a way to safety and simply deal with angle wrap with the minimum number of case statements.

Angle wrap occurs when using a particular representation for angle (either 0-360 deg or -180 - 180 deg (or equivalent in radians)) and you wrap over the angle. For example say you have an angle of -170, and you subtract 50 deg. You mathematically add up to -220 but should actually be +140 deg.

Obviously you can check for this using:

if (deg < -180) { 180 - abs(deg + 180); }

or similar. But firstly you need multitudes of checks and secondly it doesn't work if you wrap twice.

The second case where this is prevalent is in the interpolation between two angles.

For Example, say I have an angle of -170 deg and 160 deg and I want halfway in between them. A common way to do this is ang1 + 0.5(ang2-ang1) but in the example i have provided it will cause the angle to be -5 deg when it should be 175.

Is there a common way to handle angle wrap in these scenarios?

Solution

For completeness I'll include both [0, 360) and [-180, 180) normalizations.

You will need #include <math.h>.

Normalize to [0,360):

double constrainAngle(double x){
    x = fmod(x,360);
    if (x < 0)
        x += 360;
    return x;
}

Normalize to [-180,180):

double constrainAngle(double x){
    x = fmod(x + 180,360);
    if (x < 0)
        x += 360;
    return x - 180;
}

The pattern should be easy enough to recognize to generalize to radians.

Angle Bisection:

double angleDiff(double a,double b){
    double dif = fmod(b - a + 180,360);
    if (dif < 0)
        dif += 360;
    return dif - 180;
}
double bisectAngle(double a,double b){
    return constrainAngle(a + angleDiff(a,b) * 0.5);
}

This should bisect an angle on the "smaller" side. (warning: not fully tested)