src/gromacs/simd/math_x86_sse4_1_single.h

   1 /*
   2  * This file is part of the GROMACS molecular simulation package.
   3  *
   4  * Copyright (c) 2012,2013, by the GROMACS development team, led by
   5  * Mark Abraham, David van der Spoel, Berk Hess, and Erik Lindahl,
   6  * and including many others, as listed in the AUTHORS file in the
   7  * top-level source directory and at http://www.gromacs.org.
   8  *
   9  * GROMACS is free software; you can redistribute it and/or
  10  * modify it under the terms of the GNU Lesser General Public License
  11  * as published by the Free Software Foundation; either version 2.1
  12  * of the License, or (at your option) any later version.
  13  *
  14  * GROMACS is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  * Lesser General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU Lesser General Public
  20  * License along with GROMACS; if not, see
  21  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
  22  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
  23  *
  24  * If you want to redistribute modifications to GROMACS, please
  25  * consider that scientific software is very special. Version
  26  * control is crucial - bugs must be traceable. We will be happy to
  27  * consider code for inclusion in the official distribution, but
  28  * derived work must not be called official GROMACS. Details are found
  29  * in the README & COPYING files - if they are missing, get the
  30  * official version at http://www.gromacs.org.
  31  *
  32  * To help us fund GROMACS development, we humbly ask that you cite
  33  * the research papers on the package. Check out http://www.gromacs.org.
  34  */
  35 #ifndef GMX_SIMD_MATH_SSE4_1_SINGLE_H
  36 #define GMX_SIMD_MATH_SSE4_1_SINGLE_H
  37
  38 #include <stdio.h>
  39 #include <math.h>
  40
  41 #include "general_x86_sse4_1.h"
  42
  43
  44
  45 #ifndef M_PI
  46 #  define M_PI 3.14159265358979323846264338327950288
  47 #endif
  48
  49
  50
  51
  52 /************************
  53  *                      *
  54  * Simple math routines *
  55  *                      *
  56  ************************/
  57
  58 /* 1.0/sqrt(x) */
  59 static gmx_inline __m128
  60 gmx_mm_invsqrt_ps(__m128 x)
  61 {
  62     const __m128 half  = _mm_set_ps(0.5, 0.5, 0.5, 0.5);
  63     const __m128 three = _mm_set_ps(3.0, 3.0, 3.0, 3.0);
  64
  65     __m128       lu = _mm_rsqrt_ps(x);
  66
  67     return _mm_mul_ps(half, _mm_mul_ps(_mm_sub_ps(three, _mm_mul_ps(_mm_mul_ps(lu, lu), x)), lu));
  68 }
  69
  70 /* sqrt(x) - Do NOT use this (but rather invsqrt) if you actually need 1.0/sqrt(x) */
  71 static gmx_inline __m128
  72 gmx_mm_sqrt_ps(__m128 x)
  73 {
  74     __m128 mask;
  75     __m128 res;
  76
  77     mask = _mm_cmpeq_ps(x, _mm_setzero_ps());
  78     res  = _mm_andnot_ps(mask, gmx_mm_invsqrt_ps(x));
  79
  80     res  = _mm_mul_ps(x, res);
  81
  82     return res;
  83 }
  84
  85 /* 1.0/x */
  86 static gmx_inline __m128
  87 gmx_mm_inv_ps(__m128 x)
  88 {
  89     const __m128 two = _mm_set_ps(2.0f, 2.0f, 2.0f, 2.0f);
  90
  91     __m128       lu = _mm_rcp_ps(x);
  92
  93     return _mm_mul_ps(lu, _mm_sub_ps(two, _mm_mul_ps(lu, x)));
  94 }
  95
  96 static gmx_inline __m128
  97 gmx_mm_abs_ps(__m128 x)
  98 {
  99     const __m128 signmask  = gmx_mm_castsi128_ps( _mm_set1_epi32(0x7FFFFFFF) );
 100
 101     return _mm_and_ps(x, signmask);
 102 }
 103
 104
 105
 106 static __m128
 107 gmx_mm_log_ps(__m128 x)
 108 {
 109     /* Same algorithm as cephes library */
 110     const __m128  expmask    = gmx_mm_castsi128_ps( _mm_set_epi32(0x7F800000, 0x7F800000, 0x7F800000, 0x7F800000) );
 111     const __m128i expbase_m1 = _mm_set1_epi32(127-1); /* We want non-IEEE format */
 112     const __m128  half       = _mm_set1_ps(0.5f);
 113     const __m128  one        = _mm_set1_ps(1.0f);
 114     const __m128  invsq2     = _mm_set1_ps(1.0f/sqrt(2.0f));
 115     const __m128  corr1      = _mm_set1_ps(-2.12194440e-4f);
 116     const __m128  corr2      = _mm_set1_ps(0.693359375f);
 117
 118     const __m128  CA_1        = _mm_set1_ps(0.070376836292f);
 119     const __m128  CB_0        = _mm_set1_ps(1.6714950086782716f);
 120     const __m128  CB_1        = _mm_set1_ps(-2.452088066061482f);
 121     const __m128  CC_0        = _mm_set1_ps(1.5220770854701728f);
 122     const __m128  CC_1        = _mm_set1_ps(-1.3422238433233642f);
 123     const __m128  CD_0        = _mm_set1_ps(1.386218787509749f);
 124     const __m128  CD_1        = _mm_set1_ps(0.35075468953796346f);
 125     const __m128  CE_0        = _mm_set1_ps(1.3429983063133937f);
 126     const __m128  CE_1        = _mm_set1_ps(1.807420826584643f);
 127
 128     __m128        fexp;
 129     __m128i       iexp;
 130     __m128        mask;
 131     __m128        x2;
 132     __m128        y;
 133     __m128        pA, pB, pC, pD, pE, tB, tC, tD, tE;
 134
 135     /* Separate x into exponent and mantissa, with a mantissa in the range [0.5..1[ (not IEEE754 standard!) */
 136     fexp  = _mm_and_ps(x, expmask);
 137     iexp  = gmx_mm_castps_si128(fexp);
 138     iexp  = _mm_srli_epi32(iexp, 23);
 139     iexp  = _mm_sub_epi32(iexp, expbase_m1);
 140
 141     x     = _mm_andnot_ps(expmask, x);
 142     x     = _mm_or_ps(x, one);
 143     x     = _mm_mul_ps(x, half);
 144
 145     mask  = _mm_cmplt_ps(x, invsq2);
 146
 147     x     = _mm_add_ps(x, _mm_and_ps(mask, x));
 148     x     = _mm_sub_ps(x, one);
 149     iexp  = _mm_add_epi32(iexp, gmx_mm_castps_si128(mask)); /* 0xFFFFFFFF = -1 as int */
 150
 151     x2    = _mm_mul_ps(x, x);
 152
 153     pA    = _mm_mul_ps(CA_1, x);
 154     pB    = _mm_mul_ps(CB_1, x);
 155     pC    = _mm_mul_ps(CC_1, x);
 156     pD    = _mm_mul_ps(CD_1, x);
 157     pE    = _mm_mul_ps(CE_1, x);
 158     tB    = _mm_add_ps(CB_0, x2);
 159     tC    = _mm_add_ps(CC_0, x2);
 160     tD    = _mm_add_ps(CD_0, x2);
 161     tE    = _mm_add_ps(CE_0, x2);
 162     pB    = _mm_add_ps(pB, tB);
 163     pC    = _mm_add_ps(pC, tC);
 164     pD    = _mm_add_ps(pD, tD);
 165     pE    = _mm_add_ps(pE, tE);
 166
 167     pA    = _mm_mul_ps(pA, pB);
 168     pC    = _mm_mul_ps(pC, pD);
 169     pE    = _mm_mul_ps(pE, x2);
 170     pA    = _mm_mul_ps(pA, pC);
 171     y     = _mm_mul_ps(pA, pE);
 172
 173     fexp  = _mm_cvtepi32_ps(iexp);
 174     y     = _mm_add_ps(y, _mm_mul_ps(fexp, corr1));
 175
 176     y     = _mm_sub_ps(y, _mm_mul_ps(half, x2));
 177     x2    = _mm_add_ps(x, y);
 178
 179     x2    = _mm_add_ps(x2, _mm_mul_ps(fexp, corr2));
 180
 181     return x2;
 182 }
 183
 184
 185 /*
 186  * 2^x function.
 187  *
 188  * The 2^w term is calculated from a (6,0)-th order (no denominator) Minimax polynomia on the interval
 189  * [-0.5,0.5]. The coefficiencts of this was derived in Mathematica using the command:
 190  *
 191  * MiniMaxApproximation[(2^x), {x, {-0.5, 0.5}, 6, 0}, WorkingPrecision -> 15]
 192  *
 193  * The largest-magnitude exponent we can represent in IEEE single-precision binary format
 194  * is 2^-126 for small numbers and 2^127 for large ones. To avoid wrap-around problems, we set the
 195  * result to zero if the argument falls outside this range. For small numbers this is just fine, but
 196  * for large numbers you could be fancy and return the smallest/largest IEEE single-precision
 197  * number instead. That would take a few extra cycles and not really help, since something is
 198  * wrong if you are using single precision to work with numbers that cannot really be represented
 199  * in single precision.
 200  *
 201  * The accuracy is at least 23 bits.
 202  */
 203 static __m128
 204 gmx_mm_exp2_ps(__m128 x)
 205 {
 206     /* Lower bound: We do not allow numbers that would lead to an IEEE fp representation exponent smaller than -126. */
 207     const __m128  arglimit = _mm_set1_ps(126.0f);
 208
 209     const __m128i expbase  = _mm_set1_epi32(127);
 210     const __m128  CA6      = _mm_set1_ps(1.535336188319500E-004);
 211     const __m128  CA5      = _mm_set1_ps(1.339887440266574E-003);
 212     const __m128  CA4      = _mm_set1_ps(9.618437357674640E-003);
 213     const __m128  CA3      = _mm_set1_ps(5.550332471162809E-002);
 214     const __m128  CA2      = _mm_set1_ps(2.402264791363012E-001);
 215     const __m128  CA1      = _mm_set1_ps(6.931472028550421E-001);
 216     const __m128  CA0      = _mm_set1_ps(1.0f);
 217
 218     __m128        valuemask;
 219     __m128i       iexppart;
 220     __m128        fexppart;
 221     __m128        intpart;
 222     __m128        x2;
 223     __m128        p0, p1;
 224
 225     iexppart  = _mm_cvtps_epi32(x);
 226     intpart   = _mm_round_ps(x, _MM_FROUND_TO_NEAREST_INT);
 227     iexppart  = _mm_slli_epi32(_mm_add_epi32(iexppart, expbase), 23);
 228     valuemask = _mm_cmpge_ps(arglimit, gmx_mm_abs_ps(x));
 229     fexppart  = _mm_and_ps(valuemask, gmx_mm_castsi128_ps(iexppart));
 230
 231     x         = _mm_sub_ps(x, intpart);
 232     x2        = _mm_mul_ps(x, x);
 233
 234     p0        = _mm_mul_ps(CA6, x2);
 235     p1        = _mm_mul_ps(CA5, x2);
 236     p0        = _mm_add_ps(p0, CA4);
 237     p1        = _mm_add_ps(p1, CA3);
 238     p0        = _mm_mul_ps(p0, x2);
 239     p1        = _mm_mul_ps(p1, x2);
 240     p0        = _mm_add_ps(p0, CA2);
 241     p1        = _mm_add_ps(p1, CA1);
 242     p0        = _mm_mul_ps(p0, x2);
 243     p1        = _mm_mul_ps(p1, x);
 244     p0        = _mm_add_ps(p0, CA0);
 245     p0        = _mm_add_ps(p0, p1);
 246     x         = _mm_mul_ps(p0, fexppart);
 247
 248     return x;
 249 }
 250
 251
 252 /* Exponential function. This could be calculated from 2^x as Exp(x)=2^(y), where y=log2(e)*x,
 253  * but there will then be a small rounding error since we lose some precision due to the
 254  * multiplication. This will then be magnified a lot by the exponential.
 255  *
 256  * Instead, we calculate the fractional part directly as a minimax approximation of
 257  * Exp(z) on [-0.5,0.5]. We use extended precision arithmetics to calculate the fraction
 258  * remaining after 2^y, which avoids the precision-loss.
 259  * The final result is correct to within 1 LSB over the entire argument range.
 260  */
 261 static __m128
 262 gmx_mm_exp_ps(__m128 x)
 263 {
 264     const __m128  argscale      = _mm_set1_ps(1.44269504088896341f);
 265     /* Lower bound: Disallow numbers that would lead to an IEEE fp exponent reaching +-127. */
 266     const __m128  arglimit      = _mm_set1_ps(126.0f);
 267     const __m128i expbase       = _mm_set1_epi32(127);
 268
 269     const __m128  invargscale0  = _mm_set1_ps(0.693359375f);
 270     const __m128  invargscale1  = _mm_set1_ps(-2.12194440e-4f);
 271
 272     const __m128  CC5           = _mm_set1_ps(1.9875691500e-4f);
 273     const __m128  CC4           = _mm_set1_ps(1.3981999507e-3f);
 274     const __m128  CC3           = _mm_set1_ps(8.3334519073e-3f);
 275     const __m128  CC2           = _mm_set1_ps(4.1665795894e-2f);
 276     const __m128  CC1           = _mm_set1_ps(1.6666665459e-1f);
 277     const __m128  CC0           = _mm_set1_ps(5.0000001201e-1f);
 278     const __m128  one           = _mm_set1_ps(1.0f);
 279
 280     __m128        y, x2;
 281     __m128        p0, p1;
 282     __m128        valuemask;
 283     __m128i       iexppart;
 284     __m128        fexppart;
 285     __m128        intpart;
 286
 287     y = _mm_mul_ps(x, argscale);
 288
 289     iexppart  = _mm_cvtps_epi32(y);
 290     intpart   = _mm_round_ps(y, _MM_FROUND_TO_NEAREST_INT);
 291
 292     iexppart  = _mm_slli_epi32(_mm_add_epi32(iexppart, expbase), 23);
 293     valuemask = _mm_cmpge_ps(arglimit, gmx_mm_abs_ps(y));
 294     fexppart  = _mm_and_ps(valuemask, gmx_mm_castsi128_ps(iexppart));
 295
 296     /* Extended precision arithmetics */
 297     x         = _mm_sub_ps(x, _mm_mul_ps(invargscale0, intpart));
 298     x         = _mm_sub_ps(x, _mm_mul_ps(invargscale1, intpart));
 299
 300     x2        = _mm_mul_ps(x, x);
 301
 302     p1        = _mm_mul_ps(CC5, x2);
 303     p0        = _mm_mul_ps(CC4, x2);
 304     p1        = _mm_add_ps(p1, CC3);
 305     p0        = _mm_add_ps(p0, CC2);
 306     p1        = _mm_mul_ps(p1, x2);
 307     p0        = _mm_mul_ps(p0, x2);
 308     p1        = _mm_add_ps(p1, CC1);
 309     p0        = _mm_add_ps(p0, CC0);
 310     p1        = _mm_mul_ps(p1, x);
 311     p0        = _mm_add_ps(p0, p1);
 312     p0        = _mm_mul_ps(p0, x2);
 313     x         = _mm_add_ps(x, one);
 314     x         = _mm_add_ps(x, p0);
 315
 316     x         = _mm_mul_ps(x, fexppart);
 317
 318     return x;
 319 }
 320
 321 /* FULL precision. Only errors in LSB */
 322 static __m128
 323 gmx_mm_erf_ps(__m128 x)
 324 {
 325     /* Coefficients for minimax approximation of erf(x)=x*P(x^2) in range [-1,1] */
 326     const __m128  CA6      = _mm_set1_ps(7.853861353153693e-5f);
 327     const __m128  CA5      = _mm_set1_ps(-8.010193625184903e-4f);
 328     const __m128  CA4      = _mm_set1_ps(5.188327685732524e-3f);
 329     const __m128  CA3      = _mm_set1_ps(-2.685381193529856e-2f);
 330     const __m128  CA2      = _mm_set1_ps(1.128358514861418e-1f);
 331     const __m128  CA1      = _mm_set1_ps(-3.761262582423300e-1f);
 332     const __m128  CA0      = _mm_set1_ps(1.128379165726710f);
 333     /* Coefficients for minimax approximation of erfc(x)=Exp(-x^2)*P((1/(x-1))^2) in range [0.67,2] */
 334     const __m128  CB9      = _mm_set1_ps(-0.0018629930017603923f);
 335     const __m128  CB8      = _mm_set1_ps(0.003909821287598495f);
 336     const __m128  CB7      = _mm_set1_ps(-0.0052094582210355615f);
 337     const __m128  CB6      = _mm_set1_ps(0.005685614362160572f);
 338     const __m128  CB5      = _mm_set1_ps(-0.0025367682853477272f);
 339     const __m128  CB4      = _mm_set1_ps(-0.010199799682318782f);
 340     const __m128  CB3      = _mm_set1_ps(0.04369575504816542f);
 341     const __m128  CB2      = _mm_set1_ps(-0.11884063474674492f);
 342     const __m128  CB1      = _mm_set1_ps(0.2732120154030589f);
 343     const __m128  CB0      = _mm_set1_ps(0.42758357702025784f);
 344     /* Coefficients for minimax approximation of erfc(x)=Exp(-x^2)*(1/x)*P((1/x)^2) in range [2,9.19] */
 345     const __m128  CC10     = _mm_set1_ps(-0.0445555913112064f);
 346     const __m128  CC9      = _mm_set1_ps(0.21376355144663348f);
 347     const __m128  CC8      = _mm_set1_ps(-0.3473187200259257f);
 348     const __m128  CC7      = _mm_set1_ps(0.016690861551248114f);
 349     const __m128  CC6      = _mm_set1_ps(0.7560973182491192f);
 350     const __m128  CC5      = _mm_set1_ps(-1.2137903600145787f);
 351     const __m128  CC4      = _mm_set1_ps(0.8411872321232948f);
 352     const __m128  CC3      = _mm_set1_ps(-0.08670413896296343f);
 353     const __m128  CC2      = _mm_set1_ps(-0.27124782687240334f);
 354     const __m128  CC1      = _mm_set1_ps(-0.0007502488047806069f);
 355     const __m128  CC0      = _mm_set1_ps(0.5642114853803148f);
 356
 357     /* Coefficients for expansion of exp(x) in [0,0.1] */
 358     /* CD0 and CD1 are both 1.0, so no need to declare them separately */
 359     const __m128  CD2      = _mm_set1_ps(0.5000066608081202f);
 360     const __m128  CD3      = _mm_set1_ps(0.1664795422874624f);
 361     const __m128  CD4      = _mm_set1_ps(0.04379839977652482f);
 362
 363     const __m128  sieve    = gmx_mm_castsi128_ps( _mm_set1_epi32(0xfffff000) );
 364     const __m128  signbit  = gmx_mm_castsi128_ps( _mm_set1_epi32(0x80000000) );
 365     const __m128  one      = _mm_set1_ps(1.0f);
 366     const __m128  two      = _mm_set1_ps(2.0f);
 367
 368     __m128        x2, x4, y;
 369     __m128        z, q, t, t2, w, w2;
 370     __m128        pA0, pA1, pB0, pB1, pC0, pC1;
 371     __m128        expmx2, corr;
 372     __m128        res_erf, res_erfc, res;
 373     __m128        mask;
 374
 375     /* Calculate erf() */
 376     x2     = _mm_mul_ps(x, x);
 377     x4     = _mm_mul_ps(x2, x2);
 378
 379     pA0  = _mm_mul_ps(CA6, x4);
 380     pA1  = _mm_mul_ps(CA5, x4);
 381     pA0  = _mm_add_ps(pA0, CA4);
 382     pA1  = _mm_add_ps(pA1, CA3);
 383     pA0  = _mm_mul_ps(pA0, x4);
 384     pA1  = _mm_mul_ps(pA1, x4);
 385     pA0  = _mm_add_ps(pA0, CA2);
 386     pA1  = _mm_add_ps(pA1, CA1);
 387     pA0  = _mm_mul_ps(pA0, x4);
 388     pA1  = _mm_mul_ps(pA1, x2);
 389     pA0  = _mm_add_ps(pA0, pA1);
 390     pA0  = _mm_add_ps(pA0, CA0);
 391
 392     res_erf = _mm_mul_ps(x, pA0);
 393
 394     /* Calculate erfc */
 395
 396     y       = gmx_mm_abs_ps(x);
 397     t       = gmx_mm_inv_ps(y);
 398     w       = _mm_sub_ps(t, one);
 399     t2      = _mm_mul_ps(t, t);
 400     w2      = _mm_mul_ps(w, w);
 401     /*
 402      * We cannot simply calculate exp(-x2) directly in single precision, since
 403      * that will lose a couple of bits of precision due to the multiplication.
 404      * Instead, we introduce x=z+w, where the last 12 bits of precision are in w.
 405      * Then we get exp(-x2) = exp(-z2)*exp((z-x)*(z+x)).
 406      *
 407      * The only drawback with this is that it requires TWO separate exponential
 408      * evaluations, which would be horrible performance-wise. However, the argument
 409      * for the second exp() call is always small, so there we simply use a
 410      * low-order minimax expansion on [0,0.1].
 411      */
 412
 413     z       = _mm_and_ps(y, sieve);
 414     q       = _mm_mul_ps( _mm_sub_ps(z, y), _mm_add_ps(z, y) );
 415
 416     corr    = _mm_mul_ps(CD4, q);
 417     corr    = _mm_add_ps(corr, CD3);
 418     corr    = _mm_mul_ps(corr, q);
 419     corr    = _mm_add_ps(corr, CD2);
 420     corr    = _mm_mul_ps(corr, q);
 421     corr    = _mm_add_ps(corr, one);
 422     corr    = _mm_mul_ps(corr, q);
 423     corr    = _mm_add_ps(corr, one);
 424
 425     expmx2  = gmx_mm_exp_ps( _mm_or_ps( signbit, _mm_mul_ps(z, z) ) );
 426     expmx2  = _mm_mul_ps(expmx2, corr);
 427
 428     pB1  = _mm_mul_ps(CB9, w2);
 429     pB0  = _mm_mul_ps(CB8, w2);
 430     pB1  = _mm_add_ps(pB1, CB7);
 431     pB0  = _mm_add_ps(pB0, CB6);
 432     pB1  = _mm_mul_ps(pB1, w2);
 433     pB0  = _mm_mul_ps(pB0, w2);
 434     pB1  = _mm_add_ps(pB1, CB5);
 435     pB0  = _mm_add_ps(pB0, CB4);
 436     pB1  = _mm_mul_ps(pB1, w2);
 437     pB0  = _mm_mul_ps(pB0, w2);
 438     pB1  = _mm_add_ps(pB1, CB3);
 439     pB0  = _mm_add_ps(pB0, CB2);
 440     pB1  = _mm_mul_ps(pB1, w2);
 441     pB0  = _mm_mul_ps(pB0, w2);
 442     pB1  = _mm_add_ps(pB1, CB1);
 443     pB1  = _mm_mul_ps(pB1, w);
 444     pB0  = _mm_add_ps(pB0, pB1);
 445     pB0  = _mm_add_ps(pB0, CB0);
 446
 447     pC0  = _mm_mul_ps(CC10, t2);
 448     pC1  = _mm_mul_ps(CC9, t2);
 449     pC0  = _mm_add_ps(pC0, CC8);
 450     pC1  = _mm_add_ps(pC1, CC7);
 451     pC0  = _mm_mul_ps(pC0, t2);
 452     pC1  = _mm_mul_ps(pC1, t2);
 453     pC0  = _mm_add_ps(pC0, CC6);
 454     pC1  = _mm_add_ps(pC1, CC5);
 455     pC0  = _mm_mul_ps(pC0, t2);
 456     pC1  = _mm_mul_ps(pC1, t2);
 457     pC0  = _mm_add_ps(pC0, CC4);
 458     pC1  = _mm_add_ps(pC1, CC3);
 459     pC0  = _mm_mul_ps(pC0, t2);
 460     pC1  = _mm_mul_ps(pC1, t2);
 461     pC0  = _mm_add_ps(pC0, CC2);
 462     pC1  = _mm_add_ps(pC1, CC1);
 463     pC0  = _mm_mul_ps(pC0, t2);
 464     pC1  = _mm_mul_ps(pC1, t);
 465     pC0  = _mm_add_ps(pC0, pC1);
 466     pC0  = _mm_add_ps(pC0, CC0);
 467     pC0  = _mm_mul_ps(pC0, t);
 468
 469     /* SELECT pB0 or pC0 for erfc() */
 470     mask     = _mm_cmplt_ps(two, y);
 471     res_erfc = _mm_blendv_ps(pB0, pC0, mask);
 472     res_erfc = _mm_mul_ps(res_erfc, expmx2);
 473
 474     /* erfc(x<0) = 2-erfc(|x|) */
 475     mask     = _mm_cmplt_ps(x, _mm_setzero_ps());
 476     res_erfc = _mm_blendv_ps(res_erfc, _mm_sub_ps(two, res_erfc), mask);
 477
 478     /* Select erf() or erfc() */
 479     mask = _mm_cmplt_ps(y, _mm_set1_ps(0.75f));
 480     res  = _mm_blendv_ps(_mm_sub_ps(one, res_erfc), res_erf, mask);
 481
 482     return res;
 483 }
 484
 485
 486 /* FULL precision. Only errors in LSB */
 487 static __m128
 488 gmx_mm_erfc_ps(__m128 x)
 489 {
 490     /* Coefficients for minimax approximation of erf(x)=x*P(x^2) in range [-1,1] */
 491     const __m128  CA6      = _mm_set1_ps(7.853861353153693e-5f);
 492     const __m128  CA5      = _mm_set1_ps(-8.010193625184903e-4f);
 493     const __m128  CA4      = _mm_set1_ps(5.188327685732524e-3f);
 494     const __m128  CA3      = _mm_set1_ps(-2.685381193529856e-2f);
 495     const __m128  CA2      = _mm_set1_ps(1.128358514861418e-1f);
 496     const __m128  CA1      = _mm_set1_ps(-3.761262582423300e-1f);
 497     const __m128  CA0      = _mm_set1_ps(1.128379165726710f);
 498     /* Coefficients for minimax approximation of erfc(x)=Exp(-x^2)*P((1/(x-1))^2) in range [0.67,2] */
 499     const __m128  CB9      = _mm_set1_ps(-0.0018629930017603923f);
 500     const __m128  CB8      = _mm_set1_ps(0.003909821287598495f);
 501     const __m128  CB7      = _mm_set1_ps(-0.0052094582210355615f);
 502     const __m128  CB6      = _mm_set1_ps(0.005685614362160572f);
 503     const __m128  CB5      = _mm_set1_ps(-0.0025367682853477272f);
 504     const __m128  CB4      = _mm_set1_ps(-0.010199799682318782f);
 505     const __m128  CB3      = _mm_set1_ps(0.04369575504816542f);
 506     const __m128  CB2      = _mm_set1_ps(-0.11884063474674492f);
 507     const __m128  CB1      = _mm_set1_ps(0.2732120154030589f);
 508     const __m128  CB0      = _mm_set1_ps(0.42758357702025784f);
 509     /* Coefficients for minimax approximation of erfc(x)=Exp(-x^2)*(1/x)*P((1/x)^2) in range [2,9.19] */
 510     const __m128  CC10     = _mm_set1_ps(-0.0445555913112064f);
 511     const __m128  CC9      = _mm_set1_ps(0.21376355144663348f);
 512     const __m128  CC8      = _mm_set1_ps(-0.3473187200259257f);
 513     const __m128  CC7      = _mm_set1_ps(0.016690861551248114f);
 514     const __m128  CC6      = _mm_set1_ps(0.7560973182491192f);
 515     const __m128  CC5      = _mm_set1_ps(-1.2137903600145787f);
 516     const __m128  CC4      = _mm_set1_ps(0.8411872321232948f);
 517     const __m128  CC3      = _mm_set1_ps(-0.08670413896296343f);
 518     const __m128  CC2      = _mm_set1_ps(-0.27124782687240334f);
 519     const __m128  CC1      = _mm_set1_ps(-0.0007502488047806069f);
 520     const __m128  CC0      = _mm_set1_ps(0.5642114853803148f);
 521
 522     /* Coefficients for expansion of exp(x) in [0,0.1] */
 523     /* CD0 and CD1 are both 1.0, so no need to declare them separately */
 524     const __m128  CD2      = _mm_set1_ps(0.5000066608081202f);
 525     const __m128  CD3      = _mm_set1_ps(0.1664795422874624f);
 526     const __m128  CD4      = _mm_set1_ps(0.04379839977652482f);
 527
 528     const __m128  sieve    = gmx_mm_castsi128_ps( _mm_set1_epi32(0xfffff000) );
 529     const __m128  signbit  = gmx_mm_castsi128_ps( _mm_set1_epi32(0x80000000) );
 530     const __m128  one      = _mm_set1_ps(1.0f);
 531     const __m128  two      = _mm_set1_ps(2.0f);
 532
 533     __m128        x2, x4, y;
 534     __m128        z, q, t, t2, w, w2;
 535     __m128        pA0, pA1, pB0, pB1, pC0, pC1;
 536     __m128        expmx2, corr;
 537     __m128        res_erf, res_erfc, res;
 538     __m128        mask;
 539
 540     /* Calculate erf() */
 541     x2     = _mm_mul_ps(x, x);
 542     x4     = _mm_mul_ps(x2, x2);
 543
 544     pA0  = _mm_mul_ps(CA6, x4);
 545     pA1  = _mm_mul_ps(CA5, x4);
 546     pA0  = _mm_add_ps(pA0, CA4);
 547     pA1  = _mm_add_ps(pA1, CA3);
 548     pA0  = _mm_mul_ps(pA0, x4);
 549     pA1  = _mm_mul_ps(pA1, x4);
 550     pA0  = _mm_add_ps(pA0, CA2);
 551     pA1  = _mm_add_ps(pA1, CA1);
 552     pA0  = _mm_mul_ps(pA0, x4);
 553     pA1  = _mm_mul_ps(pA1, x2);
 554     pA0  = _mm_add_ps(pA0, pA1);
 555     pA0  = _mm_add_ps(pA0, CA0);
 556
 557     res_erf = _mm_mul_ps(x, pA0);
 558
 559     /* Calculate erfc */
 560     y       = gmx_mm_abs_ps(x);
 561     t       = gmx_mm_inv_ps(y);
 562     w       = _mm_sub_ps(t, one);
 563     t2      = _mm_mul_ps(t, t);
 564     w2      = _mm_mul_ps(w, w);
 565     /*
 566      * We cannot simply calculate exp(-x2) directly in single precision, since
 567      * that will lose a couple of bits of precision due to the multiplication.
 568      * Instead, we introduce x=z+w, where the last 12 bits of precision are in w.
 569      * Then we get exp(-x2) = exp(-z2)*exp((z-x)*(z+x)).
 570      *
 571      * The only drawback with this is that it requires TWO separate exponential
 572      * evaluations, which would be horrible performance-wise. However, the argument
 573      * for the second exp() call is always small, so there we simply use a
 574      * low-order minimax expansion on [0,0.1].
 575      */
 576
 577     z       = _mm_and_ps(y, sieve);
 578     q       = _mm_mul_ps( _mm_sub_ps(z, y), _mm_add_ps(z, y) );
 579
 580     corr    = _mm_mul_ps(CD4, q);
 581     corr    = _mm_add_ps(corr, CD3);
 582     corr    = _mm_mul_ps(corr, q);
 583     corr    = _mm_add_ps(corr, CD2);
 584     corr    = _mm_mul_ps(corr, q);
 585     corr    = _mm_add_ps(corr, one);
 586     corr    = _mm_mul_ps(corr, q);
 587     corr    = _mm_add_ps(corr, one);
 588
 589     expmx2  = gmx_mm_exp_ps( _mm_or_ps( signbit, _mm_mul_ps(z, z) ) );
 590     expmx2  = _mm_mul_ps(expmx2, corr);
 591
 592     pB1  = _mm_mul_ps(CB9, w2);
 593     pB0  = _mm_mul_ps(CB8, w2);
 594     pB1  = _mm_add_ps(pB1, CB7);
 595     pB0  = _mm_add_ps(pB0, CB6);
 596     pB1  = _mm_mul_ps(pB1, w2);
 597     pB0  = _mm_mul_ps(pB0, w2);
 598     pB1  = _mm_add_ps(pB1, CB5);
 599     pB0  = _mm_add_ps(pB0, CB4);
 600     pB1  = _mm_mul_ps(pB1, w2);
 601     pB0  = _mm_mul_ps(pB0, w2);
 602     pB1  = _mm_add_ps(pB1, CB3);
 603     pB0  = _mm_add_ps(pB0, CB2);
 604     pB1  = _mm_mul_ps(pB1, w2);
 605     pB0  = _mm_mul_ps(pB0, w2);
 606     pB1  = _mm_add_ps(pB1, CB1);
 607     pB1  = _mm_mul_ps(pB1, w);
 608     pB0  = _mm_add_ps(pB0, pB1);
 609     pB0  = _mm_add_ps(pB0, CB0);
 610
 611     pC0  = _mm_mul_ps(CC10, t2);
 612     pC1  = _mm_mul_ps(CC9, t2);
 613     pC0  = _mm_add_ps(pC0, CC8);
 614     pC1  = _mm_add_ps(pC1, CC7);
 615     pC0  = _mm_mul_ps(pC0, t2);
 616     pC1  = _mm_mul_ps(pC1, t2);
 617     pC0  = _mm_add_ps(pC0, CC6);
 618     pC1  = _mm_add_ps(pC1, CC5);
 619     pC0  = _mm_mul_ps(pC0, t2);
 620     pC1  = _mm_mul_ps(pC1, t2);
 621     pC0  = _mm_add_ps(pC0, CC4);
 622     pC1  = _mm_add_ps(pC1, CC3);
 623     pC0  = _mm_mul_ps(pC0, t2);
 624     pC1  = _mm_mul_ps(pC1, t2);
 625     pC0  = _mm_add_ps(pC0, CC2);
 626     pC1  = _mm_add_ps(pC1, CC1);
 627     pC0  = _mm_mul_ps(pC0, t2);
 628     pC1  = _mm_mul_ps(pC1, t);
 629     pC0  = _mm_add_ps(pC0, pC1);
 630     pC0  = _mm_add_ps(pC0, CC0);
 631     pC0  = _mm_mul_ps(pC0, t);
 632
 633     /* SELECT pB0 or pC0 for erfc() */
 634     mask     = _mm_cmplt_ps(two, y);
 635     res_erfc = _mm_blendv_ps(pB0, pC0, mask);
 636     res_erfc = _mm_mul_ps(res_erfc, expmx2);
 637
 638     /* erfc(x<0) = 2-erfc(|x|) */
 639     mask     = _mm_cmplt_ps(x, _mm_setzero_ps());
 640     res_erfc = _mm_blendv_ps(res_erfc, _mm_sub_ps(two, res_erfc), mask);
 641
 642     /* Select erf() or erfc() */
 643     mask = _mm_cmplt_ps(y, _mm_set1_ps(0.75f));
 644     res  = _mm_blendv_ps(res_erfc, _mm_sub_ps(one, res_erf), mask);
 645
 646     return res;
 647 }
 648
 649
 650 /* Calculate the force correction due to PME analytically.
 651  *
 652  * This routine is meant to enable analytical evaluation of the
 653  * direct-space PME electrostatic force to avoid tables.
 654  *
 655  * The direct-space potential should be Erfc(beta*r)/r, but there
 656  * are some problems evaluating that:
 657  *
 658  * First, the error function is difficult (read: expensive) to
 659  * approxmiate accurately for intermediate to large arguments, and
 660  * this happens already in ranges of beta*r that occur in simulations.
 661  * Second, we now try to avoid calculating potentials in Gromacs but
 662  * use forces directly.
 663  *
 664  * We can simply things slight by noting that the PME part is really
 665  * a correction to the normal Coulomb force since Erfc(z)=1-Erf(z), i.e.
 666  *
 667  * V= 1/r - Erf(beta*r)/r
 668  *
 669  * The first term we already have from the inverse square root, so
 670  * that we can leave out of this routine.
 671  *
 672  * For pme tolerances of 1e-3 to 1e-8 and cutoffs of 0.5nm to 1.8nm,
 673  * the argument beta*r will be in the range 0.15 to ~4. Use your
 674  * favorite plotting program to realize how well-behaved Erf(z)/z is
 675  * in this range!
 676  *
 677  * We approximate f(z)=erf(z)/z with a rational minimax polynomial.
 678  * However, it turns out it is more efficient to approximate f(z)/z and
 679  * then only use even powers. This is another minor optimization, since
 680  * we actually WANT f(z)/z, because it is going to be multiplied by
 681  * the vector between the two atoms to get the vectorial force. The
 682  * fastest flops are the ones we can avoid calculating!
 683  *
 684  * So, here's how it should be used:
 685  *
 686  * 1. Calculate r^2.
 687  * 2. Multiply by beta^2, so you get z^2=beta^2*r^2.
 688  * 3. Evaluate this routine with z^2 as the argument.
 689  * 4. The return value is the expression:
 690  *
 691  *
 692  *       2*exp(-z^2)     erf(z)
 693  *       ------------ - --------
 694  *       sqrt(Pi)*z^2      z^3
 695  *
 696  * 5. Multiply the entire expression by beta^3. This will get you
 697  *
 698  *       beta^3*2*exp(-z^2)     beta^3*erf(z)
 699  *       ------------------  - ---------------
 700  *          sqrt(Pi)*z^2            z^3
 701  *
 702  *    or, switching back to r (z=r*beta):
 703  *
 704  *       2*beta*exp(-r^2*beta^2)   erf(r*beta)
 705  *       ----------------------- - -----------
 706  *            sqrt(Pi)*r^2            r^3
 707  *
 708  *
 709  *    With a bit of math exercise you should be able to confirm that
 710  *    this is exactly D[Erf[beta*r]/r,r] divided by r another time.
 711  *
 712  * 6. Add the result to 1/r^3, multiply by the product of the charges,
 713  *    and you have your force (divided by r). A final multiplication
 714  *    with the vector connecting the two particles and you have your
 715  *    vectorial force to add to the particles.
 716  *
 717  */
 718 static gmx_inline __m128
 719 gmx_mm_pmecorrF_ps(__m128 z2)
 720 {
 721     const __m128  FN6      = _mm_set1_ps(-1.7357322914161492954e-8f);
 722     const __m128  FN5      = _mm_set1_ps(1.4703624142580877519e-6f);
 723     const __m128  FN4      = _mm_set1_ps(-0.000053401640219807709149f);
 724     const __m128  FN3      = _mm_set1_ps(0.0010054721316683106153f);
 725     const __m128  FN2      = _mm_set1_ps(-0.019278317264888380590f);
 726     const __m128  FN1      = _mm_set1_ps(0.069670166153766424023f);
 727     const __m128  FN0      = _mm_set1_ps(-0.75225204789749321333f);
 728
 729     const __m128  FD4      = _mm_set1_ps(0.0011193462567257629232f);
 730     const __m128  FD3      = _mm_set1_ps(0.014866955030185295499f);
 731     const __m128  FD2      = _mm_set1_ps(0.11583842382862377919f);
 732     const __m128  FD1      = _mm_set1_ps(0.50736591960530292870f);
 733     const __m128  FD0      = _mm_set1_ps(1.0f);
 734
 735     __m128        z4;
 736     __m128        polyFN0, polyFN1, polyFD0, polyFD1;
 737
 738     z4             = _mm_mul_ps(z2, z2);
 739
 740     polyFD0        = _mm_mul_ps(FD4, z4);
 741     polyFD1        = _mm_mul_ps(FD3, z4);
 742     polyFD0        = _mm_add_ps(polyFD0, FD2);
 743     polyFD1        = _mm_add_ps(polyFD1, FD1);
 744     polyFD0        = _mm_mul_ps(polyFD0, z4);
 745     polyFD1        = _mm_mul_ps(polyFD1, z2);
 746     polyFD0        = _mm_add_ps(polyFD0, FD0);
 747     polyFD0        = _mm_add_ps(polyFD0, polyFD1);
 748
 749     polyFD0        = gmx_mm_inv_ps(polyFD0);
 750
 751     polyFN0        = _mm_mul_ps(FN6, z4);
 752     polyFN1        = _mm_mul_ps(FN5, z4);
 753     polyFN0        = _mm_add_ps(polyFN0, FN4);
 754     polyFN1        = _mm_add_ps(polyFN1, FN3);
 755     polyFN0        = _mm_mul_ps(polyFN0, z4);
 756     polyFN1        = _mm_mul_ps(polyFN1, z4);
 757     polyFN0        = _mm_add_ps(polyFN0, FN2);
 758     polyFN1        = _mm_add_ps(polyFN1, FN1);
 759     polyFN0        = _mm_mul_ps(polyFN0, z4);
 760     polyFN1        = _mm_mul_ps(polyFN1, z2);
 761     polyFN0        = _mm_add_ps(polyFN0, FN0);
 762     polyFN0        = _mm_add_ps(polyFN0, polyFN1);
 763
 764     return _mm_mul_ps(polyFN0, polyFD0);
 765 }
 766
 767
 768 /* Calculate the potential correction due to PME analytically.
 769  *
 770  * See gmx_mm256_pmecorrF_ps() for details about the approximation.
 771  *
 772  * This routine calculates Erf(z)/z, although you should provide z^2
 773  * as the input argument.
 774  *
 775  * Here's how it should be used:
 776  *
 777  * 1. Calculate r^2.
 778  * 2. Multiply by beta^2, so you get z^2=beta^2*r^2.
 779  * 3. Evaluate this routine with z^2 as the argument.
 780  * 4. The return value is the expression:
 781  *
 782  *
 783  *        erf(z)
 784  *       --------
 785  *          z
 786  *
 787  * 5. Multiply the entire expression by beta and switching back to r (z=r*beta):
 788  *
 789  *       erf(r*beta)
 790  *       -----------
 791  *           r
 792  *
 793  * 6. Subtract the result from 1/r, multiply by the product of the charges,
 794  *    and you have your potential.
 795  */
 796 static gmx_inline __m128
 797 gmx_mm_pmecorrV_ps(__m128 z2)
 798 {
 799     const __m128  VN6      = _mm_set1_ps(1.9296833005951166339e-8f);
 800     const __m128  VN5      = _mm_set1_ps(-1.4213390571557850962e-6f);
 801     const __m128  VN4      = _mm_set1_ps(0.000041603292906656984871f);
 802     const __m128  VN3      = _mm_set1_ps(-0.00013134036773265025626f);
 803     const __m128  VN2      = _mm_set1_ps(0.038657983986041781264f);
 804     const __m128  VN1      = _mm_set1_ps(0.11285044772717598220f);
 805     const __m128  VN0      = _mm_set1_ps(1.1283802385263030286f);
 806
 807     const __m128  VD3      = _mm_set1_ps(0.0066752224023576045451f);
 808     const __m128  VD2      = _mm_set1_ps(0.078647795836373922256f);
 809     const __m128  VD1      = _mm_set1_ps(0.43336185284710920150f);
 810     const __m128  VD0      = _mm_set1_ps(1.0f);
 811
 812     __m128        z4;
 813     __m128        polyVN0, polyVN1, polyVD0, polyVD1;
 814
 815     z4             = _mm_mul_ps(z2, z2);
 816
 817     polyVD1        = _mm_mul_ps(VD3, z4);
 818     polyVD0        = _mm_mul_ps(VD2, z4);
 819     polyVD1        = _mm_add_ps(polyVD1, VD1);
 820     polyVD0        = _mm_add_ps(polyVD0, VD0);
 821     polyVD1        = _mm_mul_ps(polyVD1, z2);
 822     polyVD0        = _mm_add_ps(polyVD0, polyVD1);
 823
 824     polyVD0        = gmx_mm_inv_ps(polyVD0);
 825
 826     polyVN0        = _mm_mul_ps(VN6, z4);
 827     polyVN1        = _mm_mul_ps(VN5, z4);
 828     polyVN0        = _mm_add_ps(polyVN0, VN4);
 829     polyVN1        = _mm_add_ps(polyVN1, VN3);
 830     polyVN0        = _mm_mul_ps(polyVN0, z4);
 831     polyVN1        = _mm_mul_ps(polyVN1, z4);
 832     polyVN0        = _mm_add_ps(polyVN0, VN2);
 833     polyVN1        = _mm_add_ps(polyVN1, VN1);
 834     polyVN0        = _mm_mul_ps(polyVN0, z4);
 835     polyVN1        = _mm_mul_ps(polyVN1, z2);
 836     polyVN0        = _mm_add_ps(polyVN0, VN0);
 837     polyVN0        = _mm_add_ps(polyVN0, polyVN1);
 838
 839     return _mm_mul_ps(polyVN0, polyVD0);
 840 }
 841
 842
 843 static int
 844 gmx_mm_sincos_ps(__m128  x,
 845                  __m128 *sinval,
 846                  __m128 *cosval)
 847 {
 848     const __m128  two_over_pi = _mm_set1_ps(2.0/M_PI);
 849     const __m128  half        = _mm_set1_ps(0.5);
 850     const __m128  one         = _mm_set1_ps(1.0);
 851
 852     const __m128i izero      = _mm_set1_epi32(0);
 853     const __m128i ione       = _mm_set1_epi32(1);
 854     const __m128i itwo       = _mm_set1_epi32(2);
 855     const __m128i ithree     = _mm_set1_epi32(3);
 856     const __m128  signbit    = gmx_mm_castsi128_ps( _mm_set1_epi32(0x80000000) );
 857
 858     const __m128  CA1         = _mm_set1_ps(1.5703125f);
 859     const __m128  CA2         = _mm_set1_ps(4.837512969970703125e-4f);
 860     const __m128  CA3         = _mm_set1_ps(7.54978995489188216e-8f);
 861
 862     const __m128  CC0         = _mm_set1_ps(-0.0013602249f);
 863     const __m128  CC1         = _mm_set1_ps(0.0416566950f);
 864     const __m128  CC2         = _mm_set1_ps(-0.4999990225f);
 865     const __m128  CS0         = _mm_set1_ps(-0.0001950727f);
 866     const __m128  CS1         = _mm_set1_ps(0.0083320758f);
 867     const __m128  CS2         = _mm_set1_ps(-0.1666665247f);
 868
 869     __m128        y, y2;
 870     __m128        z;
 871     __m128i       iz;
 872     __m128i       offset_sin, offset_cos;
 873     __m128        tmp1, tmp2;
 874     __m128        mask_sin, mask_cos;
 875     __m128        tmp_sin, tmp_cos;
 876
 877     y          = _mm_mul_ps(x, two_over_pi);
 878     y          = _mm_add_ps(y, _mm_or_ps(_mm_and_ps(y, signbit), half));
 879
 880     iz         = _mm_cvttps_epi32(y);
 881     z          = _mm_round_ps(y, _MM_FROUND_TO_ZERO);
 882
 883     offset_sin = _mm_and_si128(iz, ithree);
 884     offset_cos = _mm_add_epi32(iz, ione);
 885
 886     /* Extended precision arithmethic to achieve full precision */
 887     y               = _mm_mul_ps(z, CA1);
 888     tmp1            = _mm_mul_ps(z, CA2);
 889     tmp2            = _mm_mul_ps(z, CA3);
 890     y               = _mm_sub_ps(x, y);
 891     y               = _mm_sub_ps(y, tmp1);
 892     y               = _mm_sub_ps(y, tmp2);
 893
 894     y2              = _mm_mul_ps(y, y);
 895
 896     tmp1            = _mm_mul_ps(CC0, y2);
 897     tmp1            = _mm_add_ps(tmp1, CC1);
 898     tmp2            = _mm_mul_ps(CS0, y2);
 899     tmp2            = _mm_add_ps(tmp2, CS1);
 900     tmp1            = _mm_mul_ps(tmp1, y2);
 901     tmp1            = _mm_add_ps(tmp1, CC2);
 902     tmp2            = _mm_mul_ps(tmp2, y2);
 903     tmp2            = _mm_add_ps(tmp2, CS2);
 904
 905     tmp1            = _mm_mul_ps(tmp1, y2);
 906     tmp1            = _mm_add_ps(tmp1, one);
 907
 908     tmp2            = _mm_mul_ps(tmp2, _mm_mul_ps(y, y2));
 909     tmp2            = _mm_add_ps(tmp2, y);
 910
 911     mask_sin        = gmx_mm_castsi128_ps(_mm_cmpeq_epi32( _mm_and_si128(offset_sin, ione), izero));
 912     mask_cos        = gmx_mm_castsi128_ps(_mm_cmpeq_epi32( _mm_and_si128(offset_cos, ione), izero));
 913
 914     tmp_sin         = _mm_blendv_ps(tmp1, tmp2, mask_sin);
 915     tmp_cos         = _mm_blendv_ps(tmp1, tmp2, mask_cos);
 916
 917     mask_sin        = gmx_mm_castsi128_ps(_mm_cmpeq_epi32( _mm_and_si128(offset_sin, itwo), izero));
 918     mask_cos        = gmx_mm_castsi128_ps(_mm_cmpeq_epi32( _mm_and_si128(offset_cos, itwo), izero));
 919
 920     tmp1            = _mm_xor_ps(signbit, tmp_sin);
 921     tmp2            = _mm_xor_ps(signbit, tmp_cos);
 922
 923     *sinval         = _mm_blendv_ps(tmp1, tmp_sin, mask_sin);
 924     *cosval         = _mm_blendv_ps(tmp2, tmp_cos, mask_cos);
 925
 926     return 0;
 927 }
 928
 929 /*
 930  * IMPORTANT: Do NOT call both sin & cos if you need both results, since each of them
 931  * will then call the sincos() routine and waste a factor 2 in performance!
 932  */
 933 static __m128
 934 gmx_mm_sin_ps(__m128 x)
 935 {
 936     __m128 s, c;
 937     gmx_mm_sincos_ps(x, &s, &c);
 938     return s;
 939 }
 940
 941 /*
 942  * IMPORTANT: Do NOT call both sin & cos if you need both results, since each of them
 943  * will then call the sincos() routine and waste a factor 2 in performance!
 944  */
 945 static __m128
 946 gmx_mm_cos_ps(__m128 x)
 947 {
 948     __m128 s, c;
 949     gmx_mm_sincos_ps(x, &s, &c);
 950     return c;
 951 }
 952
 953
 954 static __m128
 955 gmx_mm_tan_ps(__m128 x)
 956 {
 957     __m128 sinval, cosval;
 958     __m128 tanval;
 959
 960     gmx_mm_sincos_ps(x, &sinval, &cosval);
 961
 962     tanval = _mm_mul_ps(sinval, gmx_mm_inv_ps(cosval));
 963
 964     return tanval;
 965 }
 966
 967
 968 static __m128
 969 gmx_mm_asin_ps(__m128 x)
 970 {
 971     /* Same algorithm as cephes library */
 972     const __m128 signmask  = gmx_mm_castsi128_ps( _mm_set1_epi32(0x7FFFFFFF) );
 973     const __m128 limitlow  = _mm_set1_ps(1e-4f);
 974     const __m128 half      = _mm_set1_ps(0.5f);
 975     const __m128 one       = _mm_set1_ps(1.0f);
 976     const __m128 halfpi    = _mm_set1_ps(M_PI/2.0f);
 977
 978     const __m128 CC5        = _mm_set1_ps(4.2163199048E-2f);
 979     const __m128 CC4        = _mm_set1_ps(2.4181311049E-2f);
 980     const __m128 CC3        = _mm_set1_ps(4.5470025998E-2f);
 981     const __m128 CC2        = _mm_set1_ps(7.4953002686E-2f);
 982     const __m128 CC1        = _mm_set1_ps(1.6666752422E-1f);
 983
 984     __m128       sign;
 985     __m128       mask;
 986     __m128       xabs;
 987     __m128       z, z1, z2, q, q1, q2;
 988     __m128       pA, pB;
 989
 990     sign  = _mm_andnot_ps(signmask, x);
 991     xabs  = _mm_and_ps(x, signmask);
 992
 993     mask  = _mm_cmpgt_ps(xabs, half);
 994
 995     z1    = _mm_mul_ps(half, _mm_sub_ps(one, xabs));
 996     q1    = _mm_mul_ps(z1, gmx_mm_invsqrt_ps(z1));
 997     q1    = _mm_andnot_ps(_mm_cmpeq_ps(xabs, one), q1);
 998
 999     q2    = xabs;
1000     z2    = _mm_mul_ps(q2, q2);
1001
1002     z     = _mm_or_ps( _mm_and_ps(mask, z1), _mm_andnot_ps(mask, z2) );
1003     q     = _mm_or_ps( _mm_and_ps(mask, q1), _mm_andnot_ps(mask, q2) );
1004
1005     z2    = _mm_mul_ps(z, z);
1006
1007     pA    = _mm_mul_ps(CC5, z2);
1008     pB    = _mm_mul_ps(CC4, z2);
1009
1010     pA    = _mm_add_ps(pA, CC3);
1011     pB    = _mm_add_ps(pB, CC2);
1012
1013     pA    = _mm_mul_ps(pA, z2);
1014     pB    = _mm_mul_ps(pB, z2);
1015
1016     pA    = _mm_add_ps(pA, CC1);
1017     pA    = _mm_mul_ps(pA, z);
1018
1019     z     = _mm_add_ps(pA, pB);
1020     z     = _mm_mul_ps(z, q);
1021     z     = _mm_add_ps(z, q);
1022
1023     q2    = _mm_sub_ps(halfpi, z);
1024     q2    = _mm_sub_ps(q2, z);
1025
1026     z     = _mm_or_ps( _mm_and_ps(mask, q2), _mm_andnot_ps(mask, z) );
1027
1028     mask  = _mm_cmpgt_ps(xabs, limitlow);
1029     z     = _mm_or_ps( _mm_and_ps(mask, z), _mm_andnot_ps(mask, xabs) );
1030
1031     z = _mm_xor_ps(z, sign);
1032
1033     return z;
1034 }
1035
1036
1037 static __m128
1038 gmx_mm_acos_ps(__m128 x)
1039 {
1040     const __m128 signmask  = gmx_mm_castsi128_ps( _mm_set1_epi32(0x7FFFFFFF) );
1041     const __m128 one_ps    = _mm_set1_ps(1.0f);
1042     const __m128 half_ps   = _mm_set1_ps(0.5f);
1043     const __m128 pi_ps     = _mm_set1_ps(M_PI);
1044     const __m128 halfpi_ps = _mm_set1_ps(M_PI/2.0f);
1045
1046     __m128       mask1;
1047     __m128       mask2;
1048     __m128       xabs;
1049     __m128       z, z1, z2, z3;
1050
1051     xabs  = _mm_and_ps(x, signmask);
1052     mask1 = _mm_cmpgt_ps(xabs, half_ps);
1053     mask2 = _mm_cmpgt_ps(x, _mm_setzero_ps());
1054
1055     z     = _mm_mul_ps(half_ps, _mm_sub_ps(one_ps, xabs));
1056     z     = _mm_mul_ps(z, gmx_mm_invsqrt_ps(z));
1057     z     = _mm_andnot_ps(_mm_cmpeq_ps(xabs, one_ps), z);
1058
1059     z     = _mm_blendv_ps(x, z, mask1);
1060     z     = gmx_mm_asin_ps(z);
1061
1062     z2    = _mm_add_ps(z, z);
1063     z1    = _mm_sub_ps(pi_ps, z2);
1064     z3    = _mm_sub_ps(halfpi_ps, z);
1065
1066     z     = _mm_blendv_ps(z1, z2, mask2);
1067     z     = _mm_blendv_ps(z3, z, mask1);
1068
1069     return z;
1070 }
1071
1072
1073 static __m128
1074 gmx_mm_atan_ps(__m128 x)
1075 {
1076     /* Same algorithm as cephes library */
1077     const __m128 signmask  = gmx_mm_castsi128_ps( _mm_set1_epi32(0x7FFFFFFF) );
1078     const __m128 limit1    = _mm_set1_ps(0.414213562373095f);
1079     const __m128 limit2    = _mm_set1_ps(2.414213562373095f);
1080     const __m128 quarterpi = _mm_set1_ps(0.785398163397448f);
1081     const __m128 halfpi    = _mm_set1_ps(1.570796326794896f);
1082     const __m128 mone      = _mm_set1_ps(-1.0f);
1083     const __m128 CC3       = _mm_set1_ps(-3.33329491539E-1f);
1084     const __m128 CC5       = _mm_set1_ps(1.99777106478E-1f);
1085     const __m128 CC7       = _mm_set1_ps(-1.38776856032E-1);
1086     const __m128 CC9       = _mm_set1_ps(8.05374449538e-2f);
1087
1088     __m128       sign;
1089     __m128       mask1, mask2;
1090     __m128       y, z1, z2;
1091     __m128       x2, x4;
1092     __m128       sum1, sum2;
1093
1094     sign  = _mm_andnot_ps(signmask, x);
1095     x     = _mm_and_ps(x, signmask);
1096
1097     mask1 = _mm_cmpgt_ps(x, limit1);
1098     mask2 = _mm_cmpgt_ps(x, limit2);
1099
1100     z1    = _mm_mul_ps(_mm_add_ps(x, mone), gmx_mm_inv_ps(_mm_sub_ps(x, mone)));
1101     z2    = _mm_mul_ps(mone, gmx_mm_inv_ps(x));
1102
1103     y     = _mm_and_ps(mask1, quarterpi);
1104     y     = _mm_blendv_ps(y, halfpi, mask2);
1105
1106     x     = _mm_blendv_ps(x, z1, mask1);
1107     x     = _mm_blendv_ps(x, z2, mask2);
1108
1109     x2    = _mm_mul_ps(x, x);
1110     x4    = _mm_mul_ps(x2, x2);
1111
1112     sum1  = _mm_mul_ps(CC9, x4);
1113     sum2  = _mm_mul_ps(CC7, x4);
1114     sum1  = _mm_add_ps(sum1, CC5);
1115     sum2  = _mm_add_ps(sum2, CC3);
1116     sum1  = _mm_mul_ps(sum1, x4);
1117     sum2  = _mm_mul_ps(sum2, x2);
1118
1119     sum1  = _mm_add_ps(sum1, sum2);
1120     sum1  = _mm_sub_ps(sum1, mone);
1121     sum1  = _mm_mul_ps(sum1, x);
1122     y     = _mm_add_ps(y, sum1);
1123
1124     y     = _mm_xor_ps(y, sign);
1125
1126     return y;
1127 }
1128
1129
1130 static __m128
1131 gmx_mm_atan2_ps(__m128 y, __m128 x)
1132 {
1133     const __m128 pi          = _mm_set1_ps(M_PI);
1134     const __m128 minuspi     = _mm_set1_ps(-M_PI);
1135     const __m128 halfpi      = _mm_set1_ps(M_PI/2.0);
1136     const __m128 minushalfpi = _mm_set1_ps(-M_PI/2.0);
1137
1138     __m128       z, z1, z3, z4;
1139     __m128       w;
1140     __m128       maskx_lt, maskx_eq;
1141     __m128       masky_lt, masky_eq;
1142     __m128       mask1, mask2, mask3, mask4, maskall;
1143
1144     maskx_lt  = _mm_cmplt_ps(x, _mm_setzero_ps());
1145     masky_lt  = _mm_cmplt_ps(y, _mm_setzero_ps());
1146     maskx_eq  = _mm_cmpeq_ps(x, _mm_setzero_ps());
1147     masky_eq  = _mm_cmpeq_ps(y, _mm_setzero_ps());
1148
1149     z         = _mm_mul_ps(y, gmx_mm_inv_ps(x));
1150     z         = gmx_mm_atan_ps(z);
1151
1152     mask1     = _mm_and_ps(maskx_eq, masky_lt);
1153     mask2     = _mm_andnot_ps(maskx_lt, masky_eq);
1154     mask3     = _mm_andnot_ps( _mm_or_ps(masky_lt, masky_eq), maskx_eq);
1155     mask4     = _mm_and_ps(masky_eq, maskx_lt);
1156
1157     maskall   = _mm_or_ps( _mm_or_ps(mask1, mask2), _mm_or_ps(mask3, mask4) );
1158
1159     z         = _mm_andnot_ps(maskall, z);
1160     z1        = _mm_and_ps(mask1, minushalfpi);
1161     z3        = _mm_and_ps(mask3, halfpi);
1162     z4        = _mm_and_ps(mask4, pi);
1163
1164     z         = _mm_or_ps( _mm_or_ps(z, z1), _mm_or_ps(z3, z4) );
1165
1166     mask1     = _mm_andnot_ps(masky_lt, maskx_lt);
1167     mask2     = _mm_and_ps(maskx_lt, masky_lt);
1168
1169     w         = _mm_or_ps( _mm_and_ps(mask1, pi), _mm_and_ps(mask2, minuspi) );
1170     w         = _mm_andnot_ps(maskall, w);
1171
1172     z         = _mm_add_ps(z, w);
1173
1174     return z;
1175 }
1176
1177
1178
1179 #endif