关于math：C：如何将浮点数包装到间隔[-pi，pi)


C: How to wrap a float to the interval [-pi, pi)

我正在寻找可以有效完成的一些不错的C代码：

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

我有什么选择？

相关讨论

编辑2013年4月19日：

模数函数已更新为处理aka.nice和arr_sea指出的边界情况：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
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.);
}
相关讨论

一线固定时间解决方案：

好的，如果您将[min,max)形式的第二个函数计算在内，则有两层关系，但是如果足够接近，您仍然可以将它们合并在一起。

1
2
3
4
5
6
7
8
9
10
11
12
13
/* 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()和float的作用大致相同：

  • 正数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添加到结果中。

我不知道负最大值会发生什么，但是请您自己检查！

相关讨论

math.h中也有fmod函数，但是该符号会引起麻烦，因此需要后续操作才能使结果在适当的范围内(就像您对while所做的一样)。对于deltaPhase的大值，这可能比减去/添加M_TWOPI数百次要快。

1
deltaPhase = fmod(deltaPhase, M_TWOPI);

编辑：
我没有进行大量尝试，但我认为您可以通过不同方式处理正值和负值来使用fmod：

1
2
3
4
    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;

计算时间是恒定的(与while解决方案不同，它随着deltaPhase的绝对值增加而变慢)

相关讨论

如果您的输入角度可以达到任意高的值，并且连续性很重要，那么您也可以尝试

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

对于x的高值，这将更好地保持sin(x)和cos(x)的连续性，尤其是在单精度(浮点数)中。

确实，确切值_pi-double_precision_approximation?= 1.22e-16

另一方面，大多数库/硬件在评估三角函数时都使用PI的高精度近似值来求模(尽管x86系列使用的模数很差)。

结果可能在[-pi，pi]中，您必须检查确切范围。

个人而言，我会通过系统地包装并坚持使用fmod解决方案(如boost之一)来防止任何角度达到几转。

相关讨论

我会这样做：

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

会有很大的数值误差。数值误差的最佳解决方案是存储以1 / PI或1 /(2 * PI)缩放的相位，并根据您的工作将其存储为固定点。

不要使用弧度，而应使用以1 /(2)缩放的角度，并使用modf，floor等。转换回弧度以使用库函数。

这还具有这样的效果：旋转一圈半圈与旋转一圈半圈一万圈是相同的，如果角度为弧度，则不能保证，因为您可以精确表示浮点值而不是求和表示形式：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
#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 << '
' ;
    }
    {
        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 << '
' ;
    }
    std::cout << '
';
}}
相关讨论

这是供其他人找到可以使用C ++和Boost的问题的版本：

1
2
3
4
5
6
7
8
9
10
11
12
13
#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依赖项：

1
2
3
4
5
6
7
8
9
10
11
#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;
}
相关讨论

我在搜索如何在两个任意数字之间包装浮点值(或双精度数)时遇到了这个问题。它没有针对我的情况专门回答，因此我制定了自己的解决方案，可以在这里看到。这将采用给定的值并将其包装在lowerBound和upperBound之间，其中up??perBound完美地与lowerBound相交，从而使它们等效(即：360度== 0度，因此360将缠绕为0)

希望这个答案对那些在寻找这个问题的人中寻找更通用的边界解决方案有所帮助。

1
2
3
4
5
6
7
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;
}

一个有关整数的问题可以在这里找到：
干净高效的算法，用于在C ++中包装整数

用于将任意角度归一化为[-π，π)的两线非迭代测试解决方案：

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

同样，对于[0，2π)：

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

如果通过截断除法实现fmod()并与被除数具有相同的符号，则可以利用它来解决一般问题：

对于(-PI，PI]：

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

对于[-PI，PI)：

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

[请注意，这是伪代码；我的原著是用Tcl编写的，我不想以此折磨所有人。我需要第一种情况，所以必须弄清楚。]

相关讨论

如果链接到glibc的libm(包括newlib的实现)，则可以访问
__ieee754_rem_pio2f()和__ieee754_rem_pio2()私有函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
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中声明的符号

在C99中：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
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;
}

建议的方式最好。对于小变形最快。如果程序中的角度不断偏移到适当的范围内，那么您应该很少遇到较大的超出范围的值。因此，每轮都要付出复杂的模块化算术代码的代价似乎是浪费的。与模块化算术(http://embeddedgurus.com/stack-overflow/2011/02/efficiency-c-tip-13-use-the-modulus-operator-with-caution/)相比，比较便宜。

我用过(在python中)：

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

相当于C代码：

1
2
3
4
5
6
7
#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域，您需要像下面这样处理：

1
2
if( angle > pi):
    angle -= 2*math.pi
相关讨论

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

相关讨论