DSP

[转]纯C实现sqrt,cos,sin,atan2

2019-07-13 19:45发布

一开始的想法就是cos,sin,atan2都可以使用泰勒级数,sqrt可以使用牛顿法。 然后。。。上网找资料。。。 首先是SQRT,这位仁兄基本思路和我一样,但是他在最后提供的这段代码的确很神奇。列在下面。 [cpp] view plain copy print?
  1. float Sqrt(float x)  
  2. {  
  3.     float xhalf = 0.5f*x;  
  4.     int i = *(int*)&x;  
  5.     i = 0x5f375a86 - (i >> 1);  
  6.     x = *(float*)&i;  
  7.     x = x*(1.5f - xhalf*x*x);  
  8.     x = x*(1.5f - xhalf*x*x);  
  9.     x = x*(1.5f - xhalf*x*x);  
  10.     return 1 / x;  
  11. }  
而后是COS和SIN,也是用泰勒级数,但是程序有点问题,我做了小小修正: [cpp] view plain copy print?
  1. float Sin(float x)  
  2. {  
  3.     int sign = 1;  
  4.     int itemCnt = 4;  
  5.     int k = 0;  
  6.     float result = 0.0f;  
  7.     float tx = 0.0f;  
  8.     int factorial = 1;  
  9.     if(x < 0)  
  10.     {  
  11.         x = -x;  
  12.         sign *= -1;  
  13.     }  
  14.     while(x >= SL_2PI)  
  15.     {  
  16.         x -= SL_2PI;  
  17.     }  
  18.     if(x > SL_PI)  
  19.     {  
  20.         x -= SL_PI;  
  21.         sign *= -1;  
  22.     }  
  23.     if(x > SL_PI_DIV_2)  
  24.     {  
  25.         x = SL_PI - x;  
  26.     }  
  27.     tx = x;  
  28.     for (k = 0; k < itemCnt; k ++)  
  29.     {  
  30.         if(k%2 == 0)  
  31.         {  
  32.             result += (tx / factorial);  
  33.         }  
  34.         else  
  35.         {  
  36.             result -= (tx / factorial);  
  37.         }  
  38.         tx *= (x * x);  
  39.         factorial *= (2*(k+1));  
  40.         factorial *= (2*(k+1) + 1);  
  41.     }  
  42.     if (1 == sign)  
  43.         return result;  
  44.     else  
  45.         return -result;  
  46. }  
atan2有点麻烦,因为sin和cos的级数收敛很快(分母为(2n+1)!和(2n)!),而atan不一样,分母为2n+1。WIKI上面有一个更加有效率的公式 但是感觉收敛效果仍旧一般,所以最终选择了积分式,代码如下: [cpp] view plain copy print?
  1. float Atan2(float y, float x, int infNum)  
  2. {  
  3.     int i;  
  4.     float z = y / x, sum = 0.0f,temp;  
  5.     float del = z / infNum;  
  6.       
  7.     for (i = 0; i < infNum;i++)  
  8.     {  
  9.         z = i*del;  
  10.         temp = 1 / (z*z + 1) * del;  
  11.         sum += temp;  
  12.     }  
  13.               
  14.     if (x>0)  
  15.     {  
  16.         return sum;  
  17.     }  
  18.     else if (y >= 0 && x < 0)  
  19.     {  
  20.         return sum + PI;  
  21.     }  
  22.     else if (y < 0 && x < 0)  
  23.     {  
  24.         return sum - PI;  
  25.     }  
  26.     else if (y > 0 && x == 0)  
  27.     {  
  28.         return PI / 2;  
  29.     }  
  30.     else if (y < 0 && x == 0)  
  31.     {  
  32.         return -1 * PI / 2;  
  33.     }  
  34.     else  
  35.     {  
  36.         return 0;  
  37.     }  
  38. }  

后面一大段if else条件判断是因为atan2和atan的区别
  这个方法虽然误差已经很小了,至少不影响我计算方向场,但是应该还有更好的。至少我在stackoverflow里面看到的是如此。比如有如此开源代码,苹果的。 http://blog.csdn.net/simitwsnyx/article/details/45890281