kylemanna
6/29/2015 - 7:55 PM

atan2.c

#include <stdio.h>
#include <math.h>
#include <stdint.h>

#define K1 (0.97239)
#define K2 (0.19195)
#define K3 ((float)((double)(1<<29) * 4.0 * K1 / M_PI))
#define K4 ((float)((double)(1<<29) * 4.0 * K2 / M_PI))

int32_t atan2i_approx2(int32_t y, int32_t x)
{
    int32_t u, v, s;
    uint32_t w;
    float f, f2, g;
    int32_t r;
    float fx, fy;
    float tt, bb;

    fx = (float) x;
    fy = (float) y;
    u = x + y;
    v = x - y;
    w = (u ^ v) >> 31;
    tt = (w) ? -fx : fy;
    bb = (w) ? fy : fx;
    f = tt / bb;
    f2 = f * f;
    g = f * (K3 - K4*f2);
    s = (((u >> 30) & 3) ^ ((v >> 31) & 1)) << 30;
    r = s + (int32_t) g;
    return r;
}

int main(int argc, char* argv[])
{
    //int a = 0xa39, b = 0x7f2;
    int a = 0xA3A, b = 0x7F6;
    //int a = 0x96E, b = 0x9A8;
    //int a = 0x6C4, b = 0x6D5;

    printf("a = %d, b = %d\n", a, b);

    double atan2_cmath = atan2(a, b);
    int32_t atan2_est = atan2i_approx2(a, b);
    double  atan2_est_rad = (double)atan2_est / (UINT32_MAX) * 2 * M_PI;
    double  atan2_est_deg = (double)atan2_est / (UINT32_MAX) * 360;

    printf("atan2          = %f rad\n", atan2_cmath);
    printf("atan2i_approx2 = %f rad\n", atan2_est_rad);
    
    
    printf("atan2          = %f deg\n", atan2_cmath * 180.0 / M_PI);
    printf("atan2i_approx2 = %f deg\n", atan2_est_deg);

    return 0;
}