常用的快速数学函数（with SSE2 )

       很明显，在工业或者一些智能设备的控制方面，对运行速度的要求总是比较严苛的，有时候，在速度和精度上做一合理的取舍就显得尤为重要了。

     今天当一个搬运工，带来一些常用的数学库函数，这些函数采用一定的算法对结果进行了高精度的逼近，在工程上应用问题不大，还有一些是采用SSE2 (单指令多数据) 加快对结果的求解。总的来说，相对于传统的数学库函数，效果还是不错的。

#define CV_PI   3.1415926535897932384626433832795

static const float atan2_p1 = 0.9997878412794807f*(float)(180/CV_PI);
static const float atan2_p3 = -0.3258083974640975f*(float)(180/CV_PI);
static const float atan2_p5 = 0.1555786518463281f*(float)(180/CV_PI);
static const float atan2_p7 = -0.04432655554792128f*(float)(180/CV_PI);

float fastAtan2( float y, float x )
{
    float ax = std::abs(x), ay = std::abs(y);
    float a, c, c2;
    if( ax >= ay )
    {
        c = ay/(ax + (float)DBL_EPSILON);
        c2 = c*c;
        a = (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
    }
    else
    {
        c = ax/(ay + (float)DBL_EPSILON);
        c2 = c*c;
        a = 90.f - (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
    }
    if( x < 0 )
        a = 180.f - a;
    if( y < 0 )
        a = 360.f - a;
    return a;
}

static void FastAtan2_32f(const float *Y, const float *X, float *angle, int len, bool angleInDegrees=true )
{
    int i = 0;
    float scale = angleInDegrees ? 1 : (float)(CV_PI/180);

#if CV_SSE2
    if( USE_SSE2 )
    {
        Cv32suf iabsmask; iabsmask.i = 0x7fffffff;
        __m128 eps = _mm_set1_ps((float)DBL_EPSILON), absmask = _mm_set1_ps(iabsmask.f);
        __m128 _90 = _mm_set1_ps(90.f), _180 = _mm_set1_ps(180.f), _360 = _mm_set1_ps(360.f);
        __m128 z = _mm_setzero_ps(), scale4 = _mm_set1_ps(scale);
        __m128 p1 = _mm_set1_ps(atan2_p1), p3 = _mm_set1_ps(atan2_p3);
        __m128 p5 = _mm_set1_ps(atan2_p5), p7 = _mm_set1_ps(atan2_p7);

        for( ; i <= len - 4; i += 4 )
        {
            __m128 x = _mm_loadu_ps(X + i), y = _mm_loadu_ps(Y + i);
            __m128 ax = _mm_and_ps(x, absmask), ay = _mm_and_ps(y, absmask);
            __m128 mask = _mm_cmplt_ps(ax, ay);
            __m128 tmin = _mm_min_ps(ax, ay), tmax = _mm_max_ps(ax, ay);
            __m128 c = _mm_div_ps(tmin, _mm_add_ps(tmax, eps));
            __m128 c2 = _mm_mul_ps(c, c);
            __m128 a = _mm_mul_ps(c2, p7);
            a = _mm_mul_ps(_mm_add_ps(a, p5), c2);
            a = _mm_mul_ps(_mm_add_ps(a, p3), c2);
            a = _mm_mul_ps(_mm_add_ps(a, p1), c);

            __m128 b = _mm_sub_ps(_90, a);
            a = _mm_xor_ps(a, _mm_and_ps(_mm_xor_ps(a, b), mask));

            b = _mm_sub_ps(_180, a);
            mask = _mm_cmplt_ps(x, z);
            a = _mm_xor_ps(a, _mm_and_ps(_mm_xor_ps(a, b), mask));

            b = _mm_sub_ps(_360, a);
            mask = _mm_cmplt_ps(y, z);
            a = _mm_xor_ps(a, _mm_and_ps(_mm_xor_ps(a, b), mask));

            a = _mm_mul_ps(a, scale4);
            _mm_storeu_ps(angle + i, a);
        }
    }
#endif

    for( ; i < len; i++ )
    {
        float x = X[i], y = Y[i];
        float ax = std::abs(x), ay = std::abs(y);
        float a, c, c2;
        if( ax >= ay )
        {
            c = ay/(ax + (float)DBL_EPSILON);
            c2 = c*c;
            a = (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
        }
        else
        {
            c = ax/(ay + (float)DBL_EPSILON);
            c2 = c*c;
            a = 90.f - (((atan2_p7*c2 + atan2_p5)*c2 + atan2_p3)*c2 + atan2_p1)*c;
        }
        if( x < 0 )
            a = 180.f - a;
        if( y < 0 )
            a = 360.f - a;
        angle[i] = (float)(a*scale);
    }
}


/* ************************************************************************** *\
   Fast cube root by Ken Turkowski
   (http://www.worldserver.com/turk/computergraphics/papers.html)
\* ************************************************************************** */
float  cubeRoot( float value )
{
    float fr;
    Cv32suf v, m;
    int ix, s;
    int ex, shx;

    v.f = value;
    ix = v.i & 0x7fffffff;
    s = v.i & 0x80000000;
    ex = (ix >> 23) - 127;
    shx = ex % 3;
    shx -= shx >= 0 ? 3 : 0;
    ex = (ex - shx) / 3; /* exponent of cube root */
    v.i = (ix & ((1<<23)-1)) | ((shx + 127)<<23);
    fr = v.f;

    /* 0.125 <= fr < 1.0 */
    /* Use quartic rational polynomial with error < 2^(-24) */
    fr = (float)(((((45.2548339756803022511987494 * fr +
    192.2798368355061050458134625) * fr +
    119.1654824285581628956914143) * fr +
    13.43250139086239872172837314) * fr +
    0.1636161226585754240958355063)/
    ((((14.80884093219134573786480845 * fr +
    151.9714051044435648658557668) * fr +
    168.5254414101568283957668343) * fr +
    33.9905941350215598754191872) * fr +
    1.0));

    /* fr *= 2^ex * sign */
    m.f = value;
    v.f = fr;
    v.i = (v.i + (ex << 23) + s) & (m.i*2 != 0 ? -1 : 0);
    return v.f;
}

   以上代码片段摘录自OpenCV (Version2.4.9)，在mathfuncs.cpp源文件当中，还有其它的一些快速函数。有需要可以去看看。

  Tips： 有些函数采用了SSE2或其它的指令集，需要包括其相应的头文件。