Calculate atan2 without std functions or C99

前端 未结 4 1661
遥遥无期
遥遥无期 2021-01-03 06:09

I am calculating angles from a 3-axis accelerometer, but my compiler doesn\'t have a atan or atan2 function. It has a reserved memory slot, but it calls a function i can\'t

相关标签:
4条回答
  • 2021-01-03 06:23

    The actual implementations of the math functions (or stubs to the HWFPU if one exists) should be in libm. With GCC this is indicated by passing -lm to the compiler, but I don't know how it is done with your specific tools.

    0 讨论(0)
  • 2021-01-03 06:29

    The following code uses a rational approximation to get the arctangent normalized to the [0 1) interval (you can multiply the result by Pi/2 to get the real arctangent)

    normalized_atan(x) ~ (b x + x^2) / (1 + 2 b x + x^2)

    where b = 0.596227

    The maximum error is 0.1620º

    #include <stdint.h>
    #include <math.h>
    
    // Approximates atan(x) normalized to the [-1,1] range
    // with a maximum error of 0.1620 degrees.
    
    float normalized_atan( float x )
    {
        static const uint32_t sign_mask = 0x80000000;
        static const float b = 0.596227f;
    
        // Extract the sign bit
        uint32_t ux_s  = sign_mask & (uint32_t &)x;
    
        // Calculate the arctangent in the first quadrant
        float bx_a = ::fabs( b * x );
        float num = bx_a + x * x;
        float atan_1q = num / ( 1.f + bx_a + num );
    
        // Restore the sign bit
        uint32_t atan_2q = ux_s | (uint32_t &)atan_1q;
        return (float &)atan_2q;
    }
    
    // Approximates atan2(y, x) normalized to the [0,4) range
    // with a maximum error of 0.1620 degrees
    
    float normalized_atan2( float y, float x )
    {
        static const uint32_t sign_mask = 0x80000000;
        static const float b = 0.596227f;
    
        // Extract the sign bits
        uint32_t ux_s  = sign_mask & (uint32_t &)x;
        uint32_t uy_s  = sign_mask & (uint32_t &)y;
    
        // Determine the quadrant offset
        float q = (float)( ( ~ux_s & uy_s ) >> 29 | ux_s >> 30 ); 
    
        // Calculate the arctangent in the first quadrant
        float bxy_a = ::fabs( b * x * y );
        float num = bxy_a + y * y;
        float atan_1q =  num / ( x * x + bxy_a + num );
    
        // Translate it to the proper quadrant
        uint32_t uatan_2q = (ux_s ^ uy_s) | (uint32_t &)atan_1q;
        return q + (float &)uatan_2q;
    } 
    

    In case you need more precision, there is a 3rd order rational function:

    normalized_atan(x) ~ ( c x + x^2 + x^3) / ( 1 + (c + 1) x + (c + 1) x^2 + x^3)

    where c = (1 + sqrt(17)) / 8

    which has a maximum approximation error of 0.00811º

    0 讨论(0)
  • 2021-01-03 06:35

    Its not very difficult to implement your own arctan2. Convert arctan2 to arctan using this formula. And you can then calculate arctan using this infinite series. If you sum sufficient number of terms of this infinite series, you will get very close to what the library function arctan2 does.

    Here is one similar implementation for exp() that you could use as a reference.

    0 讨论(0)
  • 2021-01-03 06:37

    There is an open source atan implementation here.

    0 讨论(0)
提交回复
热议问题