C：如何將浮點數包裹到區間 [-pi, pi)

Question

我正在尋找一些可以有效完成的不錯的 C 代碼：

while (deltaPhase >= M_PI) deltaPhase -= M_TWOPI;
while (deltaPhase < -M_PI) deltaPhase += M_TWOPI;

我有哪些選擇？

Answer 1

2013 年 4 月 19 日編輯：

模函數已更新以處理 aka.nice 和 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 2

單線性恆定時間解決方案：

好吧，如果你計算[min,max)形式的第二個函數，它是一個雙線，但足夠接近 - 無論如何你可以將它們合並在一起。

/* 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);
}

然后你可以簡單地使用deltaPhase = wrapMinMax(deltaPhase, -M_PI, +M_PI) 。

解決方案是固定時間的，這意味着它所花費的時間不取決於您的價值與[-PI,+PI)距離——無論好壞。

確認：

現在，我不希望你相信我的話，所以這里有一些例子，包括邊界條件。 為清楚起見，我使用整數，但它與fmod()和浮點數的工作原理大致相同：

• 正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
• 負x ：
  • 注意：這些假設整數模復制左手符號； 如果沒有，您會得到上述（“正面”）案例。
  • 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
• 邊界：
  • 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
    • 注意：對於浮點數，可能是-0而不是+0 。

wrapMinMax函數的工作原理大致相同：將x包裝到[min,max)與將x - min包裝到[0,max-min) ，然后（重新）將min添加到結果中。

我不知道負最大值會發生什么，但請自行檢查！

Answer 3

如果您的輸入角度可以達到任意高的值，並且連續性很重要，您也可以嘗試

atan2(sin(x),cos(x))

對於高 x 值，這將比模數更好地保持 sin(x) 和 cos(x) 的連續性，尤其是在單精度 (float) 中。

確實，exact_value_of_pi - double_precision_approximation ~= 1.22e-16

另一方面，大多數庫/硬件在評估三角函數時使用 PI 的高精度近似值來應用模數（盡管已知 x86 系列使用相當差的函數）。

結果可能在 [-pi,pi] 中，您必須檢查確切的界限。

就我個人而言，我會通過系統地包裹來防止任何角度達到幾圈，並堅持使用像 boost 這樣的 fmod 解決方案。

Answer 4

math.h也有fmod函數，但該符號會引起麻煩，因此需要進行后續操作以使結果在適當的范圍內（就像您已經對 while 所做的那樣）。 對於較大的deltaPhase值，這可能比減去/添加“M_TWOPI”數百倍快。

deltaPhase = fmod(deltaPhase, M_TWOPI);

編輯：我沒有深入嘗試，但我認為您可以通過以不同方式處理正值和負值來以這種方式使用fmod ：

    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;

計算時間是恆定的（與隨着 deltaPhase 的絕對值增加而變慢的 while 解決方案不同）

Answer 5

我會這樣做：

double wrap(double x) {
    return x-2*M_PI*floor(x/(2*M_PI)+0.5);  
}

會有很大的數值誤差。 數值誤差的最佳解決方案是存儲按 1/PI 或 1/(2*PI) 縮放的相位，並根據您的操作將它們存儲為定點。

Answer 6

不要使用弧度，而是使用按1/(2π)縮放的角度並使用 modf、floor 等。轉換回弧度以使用庫函數。

這也具有旋轉一萬半圈與旋轉一萬圈和一萬圈相同的效果，如果您的角度以弧度為單位，則無法保證這一點，因為您在浮點值中有准確的表示，而不是求和近似值陳述：

#include <iostream>
#include <cmath>

float wrap_rads ( float r )
{
    while ( r > M_PI ) {
        r -= 2 * M_PI;
    }

    while ( r <= -M_PI ) {
        r += 2 * M_PI;
    }

    return r;
}
float wrap_grads ( float r )
{
    float i;
    r = modff ( r, &i );

    if ( r > 0.5 ) r -= 1;
    if ( r <= -0.5 ) r += 1;

    return r;
}

int main ()
{
    for (int rotations = 1; rotations < 100000; rotations *= 10 ) {
    {
        float pi = ( float ) M_PI;
        float two_pi = 2 * pi;

        float a = pi;
        a += rotations * two_pi;

        std::cout << rotations << " and a half rotations in radians " << a << " => " << wrap_rads ( a ) / two_pi << '\n' ;
    }
    {
        float pi = ( float ) 0.5;
        float two_pi = 2 * pi;

        float a = pi;
        a += rotations * two_pi;

        std::cout << rotations << " and a half rotations in grads " << a << " => " << wrap_grads ( a ) / two_pi << '\n' ;
    }
    std::cout << '\n';
}}

Answer 7

這是其他人發現此問題的版本，可以將 C++ 與 Boost 結合使用：

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sign.hpp>

template<typename T>
inline T normalizeRadiansPiToMinusPi(T rad)
{
  // copy the sign of the value in radians to the value of pi
  T signedPI = boost::math::copysign(boost::math::constants::pi<T>(),rad);
  // set the value of rad to the appropriate signed value between pi and -pi
  rad = fmod(rad+signedPI,(2*boost::math::constants::pi<T>())) - signedPI;

  return rad;
} 

C++11 版本，無 Boost 依賴：

#include <cmath>

// Bring the 'difference' between two angles into [-pi; pi].
template <typename T>
T normalizeRadiansPiToMinusPi(T rad) {
  // Copy the sign of the value in radians to the value of pi.
  T signed_pi = std::copysign(M_PI,rad);
  // Set the value of difference to the appropriate signed value between pi and -pi.
  rad = std::fmod(rad + signed_pi,(2 * M_PI)) - signed_pi;
  return rad;
}

Answer 8

我在搜索如何在兩個任意數字之間包裝浮點值（或雙精度值）時遇到了這個問題。 它沒有專門針對我的情況回答，所以我制定了自己的解決方案，可以在這里看到。 這將取一個給定的值並將其包裝在lowerBound和upperBound之間，其中up​​perBound完美地滿足lowerBound，這樣它們是等效的（即：360度== 0度，所以360將包裝為0）

希望這個答案對在這個問題上絆倒尋找更通用的邊界解決方案的其他人有所幫助。

double boundBetween(double val, double lowerBound, double upperBound){
   if(lowerBound > upperBound){std::swap(lowerBound, upperBound);}
   val-=lowerBound; //adjust to 0
   double rangeSize = upperBound - lowerBound;
   if(rangeSize == 0){return upperBound;} //avoid dividing by 0
   return val - (rangeSize * std::floor(val/rangeSize)) + lowerBound;
}

整數的相關問題可以在這里找到： Clean, Effective algorithm for wrapping integers in C++

Answer 9

在 fmod() 通過截除法實現並且與被除數具有相同符號的情況下，可以利用它來解決一般問題：

對於 (-PI, PI] 的情況：

if (x > 0) x = x - 2PI * ceil(x/2PI)  #Shift to the negative regime
return fmod(x - PI, 2PI) + PI

對於 [-PI, PI) 的情況：

if (x < 0) x = x - 2PI * floor(x/2PI)  #Shift to the positive regime
return fmod(x + PI, 2PI) - PI

[注意這是偽代碼； 我的原作是用 Tcl 寫的，我不想用它來折磨每個人。 我需要第一個案例，所以必須弄清楚這一點。]

Answer 10

用於將任意角度歸一化為 [-π, π) 的兩行、非迭代、經過測試的解決方案：

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

類似地，對於 [0, 2π)：

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

Answer 11

我使用過（在python中）：

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

c 代碼等效：

#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;
}

請注意，這會將它帶入包裝域 +/- 2pi 中，因此對於 +/- pi 域，您需要在之后處理它，例如：

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

Answer 12

你建議的方式是最好的。 對於小偏轉，它是最快的。 如果您的程序中的角度不斷偏轉到正確的范圍內，那么您應該很少遇到大的超出范圍的值。 因此，每輪都支付復雜的模塊化算術代碼的成本似乎是浪費。 與模塊化算法相比，比較便宜（ http://embeddedgurus.com/stack-overflow/2011/02/efficient-c-tip-13-use-the-modulus-operator-with-caution/ ）。

Answer 13

在 C99 中：

float unwindRadians( float radians )
{
   const bool radiansNeedUnwinding = radians < -M_PI || M_PI <= radians;

   if ( radiansNeedUnwinding )
   {
      if ( signbit( radians ) )
      {
         radians = -fmodf( -radians + M_PI, 2.f * M_PI ) + M_PI;
      }
      else
      {
         radians = fmodf( radians + M_PI, 2.f * M_PI ) - M_PI;
      }
   }

   return radians;
}

Answer 14

如果鏈接到 glibc 的 libm（包括 newlib 的實現），您可以訪問 __ieee754_rem_pio2f() 和 __ieee754_rem_pio2() 私有函數：

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];
}

編輯：剛剛意識到您需要鏈接到 libm.a，我找不到在 libm.so 中聲明的符號

Answer 15

deltaPhase -= floor(deltaPhase/M_TWOPI)*M_TWOPI;