Dealing with Angle Wrap in c++ code

Question

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?

Answer 1

I decided to compare the output of several implementations presented here (and also measure performance), and this is what I noticed:

#include <cstdio>
#include <cmath>

// Normalise an angle to range [-180, 180)
float constrainAngle_loop(float angle_deg)
{
    while(angle_deg > 180.0f)
    {
        angle_deg -= 360.0f;
    }
    while(angle_deg < -180.0f)
    {
        angle_deg += 360.0f;
    }
    return angle_deg;
}

// Normalise an angle to range [-180, 180)
float constrainAngle_fmod(float angle_deg)
{
    angle_deg = fmodf(angle_deg + 180.0f, 360.0f);
    if (angle_deg < 0.0f)
        angle_deg += 360.0f;
    return angle_deg - 180.0f;
}

// Normalise an angle to range [-180, 180)
float constrainAngle_rem(float angle_deg)
{
    return remainderf(angle_deg, 360.0f);
}

// Normalise an angle to range [-180, 180)
float constrainAngle_floor(float angle_deg)
{
    return angle_deg -= 360.0f * floorf((angle_deg + 180.0f) * (1.0f / 360.0f));
}

int main()
{
    for(int angle = -370; angle <= 370; angle += 10)
    {
        printf("angle: %+4i; loop: %+11.6f, fmod: %+11.6f, rem: %+11.6f, floor: %+11.6f\n", 
            angle, 
            constrainAngle_loop(angle), 
            constrainAngle_fmod(angle), 
            constrainAngle_rem(angle),
            constrainAngle_floor(angle)
        );
    }    

    return 0;
}

Compiler explorer: https://godbolt.org/z/GE4fn3a95

Output:

angle: -370; loop:  -10.000000, fmod:  -10.000000, rem:  -10.000000, floor:  -10.000000
angle: -360; loop:   +0.000000, fmod:   +0.000000, rem:   -0.000000, floor:   +0.000000
angle: -350; loop:  +10.000000, fmod:  +10.000000, rem:  +10.000000, floor:  +10.000000
angle: -340; loop:  +20.000000, fmod:  +20.000000, rem:  +20.000000, floor:  +20.000000
angle: -330; loop:  +30.000000, fmod:  +30.000000, rem:  +30.000000, floor:  +30.000000
angle: -320; loop:  +40.000000, fmod:  +40.000000, rem:  +40.000000, floor:  +40.000000
angle: -310; loop:  +50.000000, fmod:  +50.000000, rem:  +50.000000, floor:  +50.000000
angle: -300; loop:  +60.000000, fmod:  +60.000000, rem:  +60.000000, floor:  +60.000000
angle: -290; loop:  +70.000000, fmod:  +70.000000, rem:  +70.000000, floor:  +70.000000
angle: -280; loop:  +80.000000, fmod:  +80.000000, rem:  +80.000000, floor:  +80.000000
angle: -270; loop:  +90.000000, fmod:  +90.000000, rem:  +90.000000, floor:  +90.000000
angle: -260; loop: +100.000000, fmod: +100.000000, rem: +100.000000, floor: +100.000000
angle: -250; loop: +110.000000, fmod: +110.000000, rem: +110.000000, floor: +110.000000
angle: -240; loop: +120.000000, fmod: +120.000000, rem: +120.000000, floor: +120.000000
angle: -230; loop: +130.000000, fmod: +130.000000, rem: +130.000000, floor: +130.000000
angle: -220; loop: +140.000000, fmod: +140.000000, rem: +140.000000, floor: +140.000000
angle: -210; loop: +150.000000, fmod: +150.000000, rem: +150.000000, floor: +150.000000
angle: -200; loop: +160.000000, fmod: +160.000000, rem: +160.000000, floor: +160.000000
angle: -190; loop: +170.000000, fmod: +170.000000, rem: +170.000000, floor: +170.000000
angle: -180; loop: -180.000000, fmod: -180.000000, rem: -180.000000, floor: -180.000000
angle: -170; loop: -170.000000, fmod: -170.000000, rem: -170.000000, floor: -170.000000
angle: -160; loop: -160.000000, fmod: -160.000000, rem: -160.000000, floor: -160.000000
angle: -150; loop: -150.000000, fmod: -150.000000, rem: -150.000000, floor: -150.000000
angle: -140; loop: -140.000000, fmod: -140.000000, rem: -140.000000, floor: -140.000000
angle: -130; loop: -130.000000, fmod: -130.000000, rem: -130.000000, floor: -130.000000
angle: -120; loop: -120.000000, fmod: -120.000000, rem: -120.000000, floor: -120.000000
angle: -110; loop: -110.000000, fmod: -110.000000, rem: -110.000000, floor: -110.000000
angle: -100; loop: -100.000000, fmod: -100.000000, rem: -100.000000, floor: -100.000000
angle:  -90; loop:  -90.000000, fmod:  -90.000000, rem:  -90.000000, floor:  -90.000000
angle:  -80; loop:  -80.000000, fmod:  -80.000000, rem:  -80.000000, floor:  -80.000000
angle:  -70; loop:  -70.000000, fmod:  -70.000000, rem:  -70.000000, floor:  -70.000000
angle:  -60; loop:  -60.000000, fmod:  -60.000000, rem:  -60.000000, floor:  -60.000000
angle:  -50; loop:  -50.000000, fmod:  -50.000000, rem:  -50.000000, floor:  -50.000000
angle:  -40; loop:  -40.000000, fmod:  -40.000000, rem:  -40.000000, floor:  -40.000000
angle:  -30; loop:  -30.000000, fmod:  -30.000000, rem:  -30.000000, floor:  -30.000000
angle:  -20; loop:  -20.000000, fmod:  -20.000000, rem:  -20.000000, floor:  -20.000000
angle:  -10; loop:  -10.000000, fmod:  -10.000000, rem:  -10.000000, floor:  -10.000000
angle:   +0; loop:   +0.000000, fmod:   +0.000000, rem:   +0.000000, floor:   +0.000000
angle:  +10; loop:  +10.000000, fmod:  +10.000000, rem:  +10.000000, floor:  +10.000000
angle:  +20; loop:  +20.000000, fmod:  +20.000000, rem:  +20.000000, floor:  +20.000000
angle:  +30; loop:  +30.000000, fmod:  +30.000000, rem:  +30.000000, floor:  +30.000000
angle:  +40; loop:  +40.000000, fmod:  +40.000000, rem:  +40.000000, floor:  +40.000000
angle:  +50; loop:  +50.000000, fmod:  +50.000000, rem:  +50.000000, floor:  +50.000000
angle:  +60; loop:  +60.000000, fmod:  +60.000000, rem:  +60.000000, floor:  +60.000000
angle:  +70; loop:  +70.000000, fmod:  +70.000000, rem:  +70.000000, floor:  +70.000000
angle:  +80; loop:  +80.000000, fmod:  +80.000000, rem:  +80.000000, floor:  +80.000000
angle:  +90; loop:  +90.000000, fmod:  +90.000000, rem:  +90.000000, floor:  +90.000000
angle: +100; loop: +100.000000, fmod: +100.000000, rem: +100.000000, floor: +100.000000
angle: +110; loop: +110.000000, fmod: +110.000000, rem: +110.000000, floor: +110.000000
angle: +120; loop: +120.000000, fmod: +120.000000, rem: +120.000000, floor: +120.000000
angle: +130; loop: +130.000000, fmod: +130.000000, rem: +130.000000, floor: +130.000000
angle: +140; loop: +140.000000, fmod: +140.000000, rem: +140.000000, floor: +140.000000
angle: +150; loop: +150.000000, fmod: +150.000000, rem: +150.000000, floor: +150.000000
angle: +160; loop: +160.000000, fmod: +160.000000, rem: +160.000000, floor: +160.000000
angle: +170; loop: +170.000000, fmod: +170.000000, rem: +170.000000, floor: +170.000000
angle: +180; loop: +180.000000, fmod: -180.000000, rem: +180.000000, floor: -180.000000
angle: +190; loop: -170.000000, fmod: -170.000000, rem: -170.000000, floor: -170.000000
angle: +200; loop: -160.000000, fmod: -160.000000, rem: -160.000000, floor: -160.000000
angle: +210; loop: -150.000000, fmod: -150.000000, rem: -150.000000, floor: -150.000000
angle: +220; loop: -140.000000, fmod: -140.000000, rem: -140.000000, floor: -140.000000
angle: +230; loop: -130.000000, fmod: -130.000000, rem: -130.000000, floor: -130.000000
angle: +240; loop: -120.000000, fmod: -120.000000, rem: -120.000000, floor: -120.000000
angle: +250; loop: -110.000000, fmod: -110.000000, rem: -110.000000, floor: -110.000000
angle: +260; loop: -100.000000, fmod: -100.000000, rem: -100.000000, floor: -100.000000
angle: +270; loop:  -90.000000, fmod:  -90.000000, rem:  -90.000000, floor:  -90.000000
angle: +280; loop:  -80.000000, fmod:  -80.000000, rem:  -80.000000, floor:  -80.000000
angle: +290; loop:  -70.000000, fmod:  -70.000000, rem:  -70.000000, floor:  -70.000000
angle: +300; loop:  -60.000000, fmod:  -60.000000, rem:  -60.000000, floor:  -60.000000
angle: +310; loop:  -50.000000, fmod:  -50.000000, rem:  -50.000000, floor:  -50.000000
angle: +320; loop:  -40.000000, fmod:  -40.000000, rem:  -40.000000, floor:  -40.000000
angle: +330; loop:  -30.000000, fmod:  -30.000000, rem:  -30.000000, floor:  -30.000000
angle: +340; loop:  -20.000000, fmod:  -20.000000, rem:  -20.000000, floor:  -20.000000
angle: +350; loop:  -10.000000, fmod:  -10.000000, rem:  -10.000000, floor:  -10.000000
angle: +360; loop:   +0.000000, fmod:   +0.000000, rem:   +0.000000, floor:   +0.000000
angle: +370; loop:  +10.000000, fmod:  +10.000000, rem:  +10.000000, floor:  +10.000000

ATTENTION: look at angle: +180; loop: +180.000000, fmod: -180.000000, rem: +180.000000, floor: -180.000000

• "fmod" and "floor" version produces correct results ("180" not inclusive, so its become "-180")!

Benchmark code:

#include <vector>
#include <cmath>

// ---------------------------------------------------------

// Normalise an angle to range [-180, 180)
float constrainAngle_loop(float angle_deg)
{
    while(angle_deg > 180.0f)
    {
        angle_deg -= 360.0f;
    }
    while(angle_deg < -180.0f)
    {
        angle_deg += 360.0f;
    }
    return angle_deg;
}

// Normalise an angle to range [-180, 180)
float constrainAngle_fmod(float angle_deg)
{
    angle_deg = fmodf(angle_deg + 180.0f, 360.0f);
    if (angle_deg < 0.0f)
        angle_deg += 360.0f;
    return angle_deg - 180.0f;
}

// Normalise an angle to range [-180, 180)
float constrainAngle_rem(float angle_deg)
{
    return remainderf(angle_deg, 360.0f);
}

// Normalise an angle to range [-180, 180)
float constrainAngle_floor(float angle_deg)
{
    return angle_deg -= 360.0f * floorf((angle_deg + 180.0f) * (1.0f / 360.0f));
}

// ---------------------------------------------------------

static void Constrain_loop(benchmark::State& state)
{
  // Code before the loop is not measured
  std::vector<float> angles;
  for(int angle = -370; angle <= 370; angle += 10)
  {
    angles.push_back(angle);
  }

  // Code inside this loop is measured repeatedly
  for (auto _ : state)
  {
    for(float angle : angles)
    {
      const float constrained = constrainAngle_loop(angle);

      // Make sure the variable is not optimized away by compiler
      benchmark::DoNotOptimize(constrained);
    }
  }
}
// Register the function as a benchmark
BENCHMARK(Constrain_loop);

// ---------------------------------------------------------

static void Constrain_fmod(benchmark::State& state)
{
  // Code before the loop is not measured
  std::vector<float> angles;
  for(int angle = -370; angle <= 370; angle += 10)
  {
    angles.push_back(angle);
  }

  // Code inside this loop is measured repeatedly
  for (auto _ : state)
  {
    for(float angle : angles)
    {
      const float constrained = constrainAngle_fmod(angle);

      // Make sure the variable is not optimized away by compiler
      benchmark::DoNotOptimize(constrained);
    }
  }
}
// Register the function as a benchmark
BENCHMARK(Constrain_fmod);

// ---------------------------------------------------------

static void Constrain_rem(benchmark::State& state)
{
  // Code before the loop is not measured
  std::vector<float> angles;
  for(int angle = -370; angle <= 370; angle += 10)
  {
    angles.push_back(angle);
  }

  // Code inside this loop is measured repeatedly
  for (auto _ : state)
  {
    for(float angle : angles)
    {
      const float constrained = constrainAngle_rem(angle);

      // Make sure the variable is not optimized away by compiler
      benchmark::DoNotOptimize(constrained);
    }
  }
}
// Register the function as a benchmark
BENCHMARK(Constrain_rem);

static void Constrain_floor(benchmark::State& state)
{
  // Code before the loop is not measured
  std::vector<float> angles;
  for(int angle = -370; angle <= 370; angle += 10)
  {
    angles.push_back(angle);
  }

  // Code inside this loop is measured repeatedly
  for (auto _ : state)
  {
    for(float angle : angles)
    {
      const float constrained = constrainAngle_floor(angle);

      // Make sure the variable is not optimized away by compiler
      benchmark::DoNotOptimize(constrained);
    }
  }
}
// Register the function as a benchmark
BENCHMARK(Constrain_floor);

Quick C++ Benchmark: https://quick-bench.com/q/7nQaaRiWQeqhnrMrSctukXzb4oU

Verdict: Depend on your needs, if range must be "open" [-180, 180) "floor" implementation is fastest, otherwise use "loop" :)

Answer 2

Try this on:

double angleRef(double thtIn, double thtRef) {
    tht = fmod(thtIn + (180-thtRef),360) + (thtRef-180);
    return tht;
}

So as in your example, if A = -170 and B = 160, then the angle halfway between them is A + 0.5*(angleRef(B,A) - A) = -185.

Or if you prefer A=160 and B=-170: A + 0.5*(angleRef(B,A) - A) = 175.

Answer 3

#include <iostream>
#include <iomanip>
#include <numbers>
#include <vector>
#include <cmath>

// Normalize to [0,2PI):
double phaseNorm(double x)
{
    x = fmod(x, 2*std::numbers::pi);
    if (x < 0)
        x += 2*std::numbers::pi;
    return x;
};

// unwrap phase [-PI,PI]
std::vector<double> phaseUnwrap(std::vector<double> in)
{
    // Normalize to [0,2PI):
    std::transform(in.begin(),in.end(),in.begin(),[&](double d){ return phaseNorm(d); });

    // unwrap iteration
    for(size_t i = 0; i < in.size()-1; ++i)
    {
        int n2PiJump = in[i] / (2*std::numbers::pi);
        in[i+1] += n2PiJump * 2*std::numbers::pi;
        if(in[i]-in[i+1] > std::numbers::pi)
        {
            in[i+1] += 2*std::numbers::pi;
        }
    }
    return in;
}

int main() {

    // create phase vector
    int n = 3;
    std::vector<double> phase;
    for(int h = 0; h < 3; ++h)
    {
        for(int i = n; i > -n; --i)
        {
            phase.push_back(-i*std::numbers::pi/n);
        }
    }

    // print phase vector
    std::cout << std::setw(25) << "Input vector: ";
    for(auto& p : phase)
    {
        std::cout << std::setw(8) << p << " ";
    }
    std::cout << std::endl;

    //  normalize phase vector
    std::cout << std::setw(25) << "Normalized vector: ";
    for(auto& p : phase)
    {
        p = phaseNorm(p);
        std::cout << std::setw(8) << p << " ";
    }
    std::cout << std::endl;

    // unwrap phase vector
    std::cout << std::setw(25) << "Unwraped norm. vector: ";
    std::vector<double> phaseUnwraped = phaseUnwrap(phase);
    for(auto& p : phaseUnwraped)
    {
        std::cout << std::setw(8) << p << " ";
    }    
    std::cout << std::endl;

    return 0;
}

           Input vector: -3.14159  -2.0944  -1.0472        0   1.0472   2.0944 -3.14159  -2.0944  -1.0472        0   1.0472   2.0944 -3.14159  -2.0944  -1.0472        0   1.0472   2.0944 
      Normalized vector:  3.14159  4.18879  5.23599        0   1.0472   2.0944  3.14159  4.18879  5.23599        0   1.0472   2.0944  3.14159  4.18879  5.23599        0   1.0472   2.0944 
  Unwraped norm. vector:  3.14159  4.18879  5.23599  6.28319  7.33038  8.37758  9.42478   10.472  11.5192  12.5664  13.6136  14.6608   15.708  16.7552  17.8024  18.8496  19.8968   20.944

To compile don't forget the c++20 flag -std=c++20 due to std::numbers::pi or use M_PI as included in <math.h>.

Answer 4

auto new_angle = atan2(sin(old_angle), cos(old_angle));

Answer 5

I find using remainder() from the math library is convenient. Given an angle a, to constrain it to -180, 180 you can just do:

remainder(a, 360.0);

and change the 360.0 to 2.0 * M_PI for radians

Answer 6

Normalise an angle to range [-180, 180)

deg -= 360. * std::floor((deg + 180.) * (1. / 360.));

Normalise an angle to range [0, 360)

deg -= 360. * std::floor(deg * (1. / 360.));

Examples:

deg = -90 -> [0, 360):

deg -= 360. * std::floor(-90 / 360.);
deg -= 360. * -1;
deg = 270

deg = 270 -> [-180, 180):

deg -= 360. * std::floor((deg + 180.) / 360.);
deg -= 360. * std::floor(480. / 360.);
deg -= 360. * 1.;
deg = -90;

See: http://en.cppreference.com/w/cpp/numeric/math/floor

Answer 7

Map angle(+PI ~ -PI) to signed int value (or short value):

angle_signed_short = angle_float / PI * 0x7FFFF;

Then you can add or sub value as normal. Then map back:

angle_float = angle_signed_short * PI / 0x7FFFF;

Answer 8

So if figured out a way to effectively do what i want using Mystical's approach to constraining the Angle. Here it is:

This seems to work with any example i can think of.

Answer 9

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)