src/math.c

   1 #include "a/math.h"
   2
   3 A_HIDDEN a_f64 a_f64_sq(a_f64 x, a_f64 *o);
   4 a_f64 a_f64_sq(a_f64 x, a_f64 *o)
   5 {
   6 #undef U
   7 #if defined(_MSC_VER) && (_MSC_VER < 1914)
   8     const union
   9     {
  10         a_u64 u;
  11         a_f64 x;
  12     } u = {A_U64_C(0x41A0000000000000)};
  13 #define U u.x
  14 #else
  15 #define U A_F64_C(0x1P27)
  16 #endif
  17     a_f64 xh, xl, xc;
  18     xc = x * (U + 1);
  19     xh = x - xc + xc;
  20     xl = x - xh;
  21     x *= x;
  22     *o = xh * xh - x + 2 * xh * xl + xl * xl;
  23     return x;
  24 #undef U
  25 }
  26
  27 #undef a_f32_hypot
  28 a_f32 a_f32_hypot(a_f32 x, a_f32 y)
  29 {
  30 #undef MIN
  31 #undef MAX
  32 #if defined(_MSC_VER) && (_MSC_VER < 1914)
  33     const union
  34     {
  35         a_u32 u;
  36         a_f32 x;
  37     } max = {A_U32_C(0x6C800000)},
  38       min = {A_U32_C(0x12800000)};
  39 #define MAX max.x
  40 #define MIN min.x
  41 #else
  42 #define MAX A_F32_C(0x1P+90)
  43 #define MIN A_F32_C(0x1P-90)
  44 #endif
  45     union
  46     {
  47         a_f32 x;
  48         a_u32 u;
  49     } ux, uy;
  50     a_f32 z;
  51
  52     ux.x = x;
  53     uy.x = y;
  54     ux.u &= A_U32_C(~0) >> 1;
  55     uy.u &= A_U32_C(~0) >> 1;
  56     if (ux.u < uy.u)
  57     {
  58         ux.u ^= uy.u;
  59         uy.u ^= ux.u;
  60         ux.u ^= uy.u;
  61     }
  62
  63     x = ux.x;
  64     y = uy.x;
  65     if (uy.u == 0xFF << 23) { return y; }
  66     if (ux.u >= 0xFF << 23 || uy.u == 0 || ux.u - uy.u >= 25 << 23) { return x + y; }
  67
  68     z = 1;
  69     if (ux.u >= (0x7F + 60) << 23)
  70     {
  71         z = MAX;
  72         x *= MIN;
  73         y *= MIN;
  74     }
  75     else if (uy.u < (0x7F - 60) << 23)
  76     {
  77         z = MIN;
  78         x *= MAX;
  79         y *= MAX;
  80     }
  81     return z * a_f32_sqrt((a_f32)((a_f64)x * (a_f64)x + (a_f64)y * (a_f64)y));
  82 #undef MIN
  83 #undef MAX
  84 }
  85
  86 #undef a_f64_hypot
  87 a_f64 a_f64_hypot(a_f64 x, a_f64 y)
  88 {
  89 #undef MIN
  90 #undef MAX
  91 #if defined(_MSC_VER) && (_MSC_VER < 1914)
  92     const union
  93     {
  94         a_u64 u;
  95         a_f64 x;
  96     } max = {A_U64_C(0x6BB0000000000000)},
  97       min = {A_U64_C(0x1430000000000000)};
  98 #define MAX max.x
  99 #define MIN min.x
 100 #else
 101 #define MAX A_F64_C(0x1P+700)
 102 #define MIN A_F64_C(0x1P-700)
 103 #endif
 104     union
 105     {
 106         a_f64 x;
 107         a_u64 u;
 108     } ux, uy;
 109     unsigned int ex, ey;
 110     a_f64 hx, lx, hy, ly, z;
 111
 112     ux.x = x;
 113     uy.x = y;
 114     ux.u &= A_U64_C(~0) >> 1;
 115     uy.u &= A_U64_C(~0) >> 1;
 116     if (ux.u < uy.u)
 117     {
 118         ux.u ^= uy.u;
 119         uy.u ^= ux.u;
 120         ux.u ^= uy.u;
 121     }
 122
 123     x = ux.x;
 124     y = uy.x;
 125     ex = (unsigned int)(ux.u >> 52);
 126     ey = (unsigned int)(uy.u >> 52);
 127     /* hypot(inf,nan) == inf */
 128     if (ey == 0x7FF) { return y; }
 129     if (ex == 0x7FF || uy.u == 0) { return x; }
 130     if (ex - ey > 64) { return x + y; }
 131
 132     z = 1;
 133     if (ex > 0x3FF + 510)
 134     {
 135         z = MAX;
 136         x *= MIN;
 137         y *= MIN;
 138     }
 139     else if (ey < 0x3FF - 450)
 140     {
 141         z = MIN;
 142         x *= MAX;
 143         y *= MAX;
 144     }
 145     hx = a_f64_sq(x, &lx);
 146     hy = a_f64_sq(y, &ly);
 147     return z * a_f64_sqrt(ly + lx + hy + hx);
 148 #undef MIN
 149 #undef MAX
 150 }
 151
 152 a_f32 a_f32_rsqrt(a_f32 x)
 153 {
 154 #if 1
 155     return 1 / a_f32_sqrt(x);
 156 #else
 157     union
 158     {
 159         a_f32 x;
 160         a_u32 u;
 161     } u;
 162     u.x = x;
 163     if (a_likely(x > 0))
 164     {
 165         a_f32 xh = A_F32_C(0.5) * x;
 166         u.u = A_U32_C(0x5F375A86) - (u.u >> 1);
 167         u.x *= A_F32_C(1.5) - u.x * u.x * xh;
 168         u.x *= A_F32_C(1.5) - u.x * u.x * xh;
 169     }
 170     else if (x < 0)
 171     {
 172         u.u = A_F32_NNAN;
 173     }
 174     else
 175     {
 176         u.u |= A_F32_PINF;
 177     }
 178     return u.x;
 179 #endif
 180 }
 181
 182 a_f64 a_f64_rsqrt(a_f64 x)
 183 {
 184 #if 1
 185     return 1 / a_f64_sqrt(x);
 186 #else
 187     union
 188     {
 189         a_f64 x;
 190         a_u64 u;
 191     } u;
 192     u.x = x;
 193     if (a_likely(x > 0))
 194     {
 195         a_f64 xh = A_F64_C(0.5) * x;
 196         u.u = A_U64_C(0x5FE6EC85E7DE30DA) - (u.u >> 1);
 197         u.x *= A_F64_C(1.5) - u.x * u.x * xh;
 198         u.x *= A_F64_C(1.5) - u.x * u.x * xh;
 199     }
 200     else if (x < 0)
 201     {
 202         u.u = A_F64_NNAN;
 203     }
 204     else
 205     {
 206         u.u |= A_F64_PINF;
 207     }
 208     return u.x;
 209 #endif
 210 }
 211
 212 a_u16 a_u32_sqrt(a_u32 x)
 213 {
 214 #if 0 /* Newton's method */
 215     a_u32 x0, x1;
 216     if (x <= 1) { return (a_u16)x; }
 217     x1 = x >> 1;
 218     do {
 219         x0 = x1;
 220         x1 = (x0 + x / x0) >> 1;
 221     } while (x0 > x1);
 222     return (a_u16)x0;
 223 #else /* Digit-by-digit */
 224     a_u32 a, b = 1, y = 0;
 225     for (b <<= sizeof(b) * 8 - 2; b > x; b >>= 2) {}
 226     for (; b; b >>= 2)
 227     {
 228         a = y + b;
 229         y >>= 1;
 230         if (x >= a)
 231         {
 232             x -= a;
 233             y |= b;
 234         }
 235     }
 236     return (a_u16)y;
 237 #endif
 238 }
 239
 240 a_u32 a_u64_sqrt(a_u64 x)
 241 {
 242 #if 0 /* Newton's method */
 243     a_u64 x0, x1;
 244     if (x <= 1) { return (a_u32)x; }
 245     x1 = x >> 1;
 246     do {
 247         x0 = x1;
 248         x1 = (x0 + x / x0) >> 1;
 249     } while (x0 > x1);
 250     return (a_u32)x0;
 251 #else /* Digit-by-digit */
 252     a_u64 a, b = 1, y = 0;
 253     for (b <<= sizeof(b) * 8 - 2; b > x; b >>= 2) {}
 254     for (; b; b >>= 2)
 255     {
 256         a = y + b;
 257         y >>= 1;
 258         if (x >= a)
 259         {
 260             x -= a;
 261             y |= b;
 262         }
 263     }
 264     return (a_u32)y;
 265 #endif
 266 }
 267
 268 #undef a_float_log1p
 269 a_float a_float_log1p(a_float x) { return a_float_log(x + 1); }
 270
 271 #undef a_float_hypot
 272 a_float a_float_hypot(a_float x, a_float y)
 273 {
 274 #if A_FLOAT_TYPE + 0 == A_FLOAT_SINGLE
 275     return a_f32_hypot(x, y);
 276 #elif A_FLOAT_TYPE + 0 == A_FLOAT_DOUBLE
 277     return a_f64_hypot(x, y);
 278 #elif A_FLOAT_TYPE + 0 == A_FLOAT_EXTEND
 279     return a_float_sqrt(x * x + y * y);
 280 #endif /* A_FLOAT_TYPE */
 281 }
 282
 283 #undef a_float_atan2
 284 a_float a_float_atan2(a_float x, a_float y)
 285 {
 286     if (x > 0) { return a_float_atan(y / x); }
 287     if (x < 0)
 288     {
 289         if (y >= 0) { return a_float_atan(y / x) + A_FLOAT_PI; }
 290         return a_float_atan(y / x) - A_FLOAT_PI;
 291     }
 292     if (y > 0) { return +A_FLOAT_PI; }
 293     if (y < 0) { return -A_FLOAT_PI; }
 294     return 0;
 295 }