角度の正規化

三角関数の引数として渡す角度は-pi~+piに収めてから渡すことが多い。

std::fmodを使って剰余を求め、その後-π~+πに収める。

inline T wrap(T x) {
    x = std::fmod(x + pi, two_pi);  // [-2π, 2π]
    if (x < 0) x += two_pi;         // [0, 2π]
    return x - pi;                  // [-π, π]
}

C++標準の関数を使うため一般的ではあるが、標準関数の実行速度が気になるため高速化実装をする例がある。

シンプルなものとしてはwhileで回し続ける。

inline T wrap(T input) {
  T temp = input;
  while (temp < -pi) {
    temp += two_pi;
  }
  while (temp > pi) {
    temp -= two_pi;
  }
  return temp;
}

軽量ではあるが、入力の値が大きくなるほど処理時間が増えるというデメリットが有る。

STM32G4(Cortex-M4)で比較してみた。 最適化をO2にしてもstd::fmodを使ったほうが処理が長いという結果なった。
±1600radくらいまではwhileのほうがはやい。それ以上だとstd::fmodのほうが早い。