lib/smmath.hh

   1 // Licensed GNU LGPL v3 or later: http://www.gnu.org/licenses/lgpl.html
   2
   3 #ifndef SPECTMORPH_MATH_HH
   4 #define SPECTMORPH_MATH_HH
   5
   6 #include <math.h>
   7 #include <glib.h>
   8 #include <string.h>
   9 #include <stdlib.h>
  10 #include <bse/bseieee754.hh>
  11 #ifdef __SSE__
  12 #include <xmmintrin.h>
  13 #endif
  14
  15 namespace SpectMorph
  16 {
  17
  18 /**
  19  * \brief parameter structure for the various optimized vector sine functions
  20  */
  21 struct
  22 VectorSinParams
  23 {
  24   double mix_freq;         //!< the mix freq (sampling rate) of the sin (and cos) wave to be created
  25   double freq;             //!< the frequency of the sin (and cos) wave to be created
  26   double phase;            //!< the start phase of the wave
  27   double mag;              //!< the magnitude (amplitude) of the wave
  28
  29   enum {
  30     NONE = -1,
  31     ADD  = 1,              //!< add computed values to the values that are in the output array
  32     REPLACE = 2            //!< replace values in the output array with computed values
  33   } mode;                  //!< whether to overwrite or add output
  34
  35   VectorSinParams() :
  36     mix_freq (-1),
  37     freq (-1),
  38     phase (-100),
  39     mag (-1),
  40     mode (NONE)
  41   {
  42   }
  43 };
  44
  45 template<class Iterator, int MODE>
  46 inline void
  47 internal_fast_vector_sin (const VectorSinParams& params, Iterator begin, Iterator end)
  48 {
  49   g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
  50
  51   const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
  52   const double inc_re = cos (phase_inc);
  53   const double inc_im = sin (phase_inc);
  54   int n = 0;
  55
  56   double state_re;
  57   double state_im;
  58
  59   sincos (params.phase, &state_im, &state_re);
  60   state_re *= params.mag;
  61   state_im *= params.mag;
  62
  63   for (Iterator x = begin; x != end; x++)
  64     {
  65       if (MODE == VectorSinParams::REPLACE)
  66         *x = state_im;
  67       else
  68         *x += state_im;
  69       if ((n++ & 255) == 255)
  70         {
  71           sincos (phase_inc * n + params.phase, &state_im, &state_re);
  72           state_re *= params.mag;
  73           state_im *= params.mag;
  74         }
  75       else
  76         {
  77           /*
  78            * (state_re + i * state_im) * (inc_re + i * inc_im) =
  79            *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
  80            */
  81           const double re = state_re * inc_re - state_im * inc_im;
  82           const double im = state_re * inc_im + state_im * inc_re;
  83           state_re = re;
  84           state_im = im;
  85         }
  86     }
  87 }
  88
  89 template<class Iterator, int MODE>
  90 inline void
  91 internal_fast_vector_sincos (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end, Iterator cos_begin)
  92 {
  93   g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
  94
  95   const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
  96   const double inc_re = cos (phase_inc);
  97   const double inc_im = sin (phase_inc);
  98   int n = 0;
  99
 100   double state_re;
 101   double state_im;
 102
 103   sincos (params.phase, &state_im, &state_re);
 104   state_re *= params.mag;
 105   state_im *= params.mag;
 106
 107   for (Iterator x = sin_begin, y = cos_begin; x != sin_end; x++, y++)
 108     {
 109       if (MODE == VectorSinParams::REPLACE)
 110         {
 111           *x = state_im;
 112           *y = state_re;
 113         }
 114       else
 115         {
 116           *x += state_im;
 117           *y += state_re;
 118         }
 119       if ((n++ & 255) == 255)
 120         {
 121           sincos (phase_inc * n + params.phase, &state_im, &state_re);
 122           state_re *= params.mag;
 123           state_im *= params.mag;
 124         }
 125       else
 126         {
 127           /*
 128            * (state_re + i * state_im) * (inc_re + i * inc_im) =
 129            *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
 130            */
 131           const double re = state_re * inc_re - state_im * inc_im;
 132           const double im = state_re * inc_im + state_im * inc_re;
 133           state_re = re;
 134           state_im = im;
 135         }
 136     }
 137 }
 138
 139 template<class Iterator>
 140 inline void
 141 fast_vector_sin (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end)
 142 {
 143   if (params.mode == VectorSinParams::ADD)
 144     {
 145       internal_fast_vector_sin<Iterator, VectorSinParams::ADD> (params, sin_begin, sin_end);
 146     }
 147   else if (params.mode == VectorSinParams::REPLACE)
 148     {
 149       internal_fast_vector_sin<Iterator, VectorSinParams::REPLACE> (params, sin_begin, sin_end);
 150     }
 151   else
 152     {
 153       g_assert_not_reached();
 154     }
 155 }
 156
 157 template<class Iterator>
 158 inline void
 159 fast_vector_sincos (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end, Iterator cos_begin)
 160 {
 161   if (params.mode == VectorSinParams::ADD)
 162     {
 163       internal_fast_vector_sincos<Iterator, VectorSinParams::ADD> (params, sin_begin, sin_end, cos_begin);
 164     }
 165   else if (params.mode == VectorSinParams::REPLACE)
 166     {
 167       internal_fast_vector_sincos<Iterator, VectorSinParams::REPLACE> (params, sin_begin, sin_end, cos_begin);
 168     }
 169   else
 170     {
 171       g_assert_not_reached();
 172     }
 173 }
 174
 175
 176 /// @cond
 177 /* see: http://ds9a.nl/gcc-simd/ */
 178 union F4Vector
 179 {
 180   float f[4];
 181 #ifdef __SSE__
 182   __m128 v;   // vector of four single floats
 183 #endif
 184 };
 185 /// @endcond
 186
 187 template<bool NEED_COS, int MODE>
 188 inline void
 189 internal_fast_vector_sincosf (const VectorSinParams& params, float *sin_begin, float *sin_end, float *cos_begin)
 190 {
 191 #ifdef __SSE__
 192   g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
 193
 194   const int TABLE_SIZE = 32;
 195
 196   const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
 197   const double inc_re16 = cos (phase_inc * TABLE_SIZE * 4);
 198   const double inc_im16 = sin (phase_inc * TABLE_SIZE * 4);
 199   int n = 0;
 200
 201   double state_re;
 202   double state_im;
 203
 204   sincos (params.phase, &state_im, &state_re);
 205   state_re *= params.mag;
 206   state_im *= params.mag;
 207
 208   F4Vector incf_re[TABLE_SIZE];
 209   F4Vector incf_im[TABLE_SIZE];
 210
 211   // compute tables using FPU
 212   VectorSinParams table_params = params;
 213   table_params.phase = 0;
 214   table_params.mag = 1;
 215   table_params.mode = VectorSinParams::REPLACE;
 216   fast_vector_sincos (table_params, incf_im[0].f, incf_im[0].f + (TABLE_SIZE * 4), incf_re[0].f);
 217
 218   // inner loop using SSE instructions
 219   int todo = sin_end - sin_begin;
 220   while (todo >= 4 * TABLE_SIZE)
 221     {
 222       F4Vector sf_re;
 223       F4Vector sf_im;
 224       sf_re.f[0] = state_re;
 225       sf_re.f[1] = state_re;
 226       sf_re.f[2] = state_re;
 227       sf_re.f[3] = state_re;
 228       sf_im.f[0] = state_im;
 229       sf_im.f[1] = state_im;
 230       sf_im.f[2] = state_im;
 231       sf_im.f[3] = state_im;
 232
 233       /*
 234        * formula for complex multiplication:
 235        *
 236        * (state_re + i * state_im) * (inc_re + i * inc_im) =
 237        *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
 238        */
 239       F4Vector *new_im = reinterpret_cast<F4Vector *> (sin_begin + n);
 240       F4Vector *new_re = reinterpret_cast<F4Vector *> (cos_begin + n);
 241       for (int k = 0; k < TABLE_SIZE; k++)
 242         {
 243           if (MODE == VectorSinParams::ADD)
 244             {
 245               if (NEED_COS)
 246                 {
 247                   new_re[k].v = _mm_add_ps (new_re[k].v, _mm_sub_ps (_mm_mul_ps (sf_re.v, incf_re[k].v),
 248                                                                      _mm_mul_ps (sf_im.v, incf_im[k].v)));
 249                 }
 250               new_im[k].v = _mm_add_ps (new_im[k].v, _mm_add_ps (_mm_mul_ps (sf_re.v, incf_im[k].v),
 251                                                      _mm_mul_ps (sf_im.v, incf_re[k].v)));
 252             }
 253           else
 254             {
 255               if (NEED_COS)
 256                 {
 257                   new_re[k].v = _mm_sub_ps (_mm_mul_ps (sf_re.v, incf_re[k].v),
 258                                             _mm_mul_ps (sf_im.v, incf_im[k].v));
 259                 }
 260               new_im[k].v = _mm_add_ps (_mm_mul_ps (sf_re.v, incf_im[k].v),
 261                                         _mm_mul_ps (sf_im.v, incf_re[k].v));
 262             }
 263         }
 264
 265       n += 4 * TABLE_SIZE;
 266
 267       /*
 268        * (state_re + i * state_im) * (inc_re + i * inc_im) =
 269        *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
 270        */
 271       const double re = state_re * inc_re16 - state_im * inc_im16;
 272       const double im = state_re * inc_im16 + state_im * inc_re16;
 273       state_re = re;
 274       state_im = im;
 275
 276       todo -= 4 * TABLE_SIZE;
 277     }
 278
 279   // compute the remaining sin/cos values using the FPU
 280   VectorSinParams rest_params = params;
 281   rest_params.phase += n * phase_inc;
 282   if (NEED_COS)
 283     fast_vector_sincos (rest_params, sin_begin + n, sin_end, cos_begin + n);
 284   else
 285     fast_vector_sin (rest_params, sin_begin + n, sin_end);
 286 #else
 287   if (NEED_COS)
 288     fast_vector_sincos (params, sin_begin, sin_end, cos_begin);
 289   else
 290     fast_vector_sin (params, sin_begin, sin_end);
 291 #endif
 292 }
 293
 294 inline void
 295 fast_vector_sincosf (const VectorSinParams& params, float *sin_begin, float *sin_end, float *cos_begin)
 296 {
 297   if (params.mode == VectorSinParams::ADD)
 298     {
 299       internal_fast_vector_sincosf<true, VectorSinParams::ADD> (params, sin_begin, sin_end, cos_begin);
 300     }
 301   else if (params.mode == VectorSinParams::REPLACE)
 302     {
 303       internal_fast_vector_sincosf<true, VectorSinParams::REPLACE> (params, sin_begin, sin_end, cos_begin);
 304     }
 305   else
 306     {
 307       g_assert_not_reached();
 308     }
 309 }
 310
 311 inline void
 312 fast_vector_sinf (const VectorSinParams& params, float *sin_begin, float *sin_end)
 313 {
 314   if (params.mode == VectorSinParams::ADD)
 315     {
 316       internal_fast_vector_sincosf<false, VectorSinParams::ADD> (params, sin_begin, sin_end, NULL);
 317     }
 318   else if (params.mode == VectorSinParams::REPLACE)
 319     {
 320       internal_fast_vector_sincosf<false, VectorSinParams::REPLACE> (params, sin_begin, sin_end, NULL);
 321     }
 322   else
 323     {
 324       g_assert_not_reached();
 325     }
 326 }
 327
 328 inline void
 329 zero_float_block (size_t n_values, float *values)
 330 {
 331   memset (values, 0, n_values * sizeof (float));
 332 }
 333
 334 inline void
 335 int_sincos (guint8 i, double *si, double *ci)
 336 {
 337   extern float *int_sincos_table;
 338
 339   *si = int_sincos_table[i];
 340   i += 64;
 341   *ci = int_sincos_table[i];
 342 }
 343
 344 inline void
 345 int_sincosf (guint8 i, float *si, float *ci)
 346 {
 347   extern float *int_sincos_table;
 348
 349   *si = int_sincos_table[i];
 350   i += 64;
 351   *ci = int_sincos_table[i];
 352 }
 353
 354 inline void
 355 int_sincos_init()
 356 {
 357   extern float *int_sincos_table;
 358
 359   int_sincos_table = (float *) malloc (sizeof (float) * 256);
 360   for (int i = 0; i < 256; i++)
 361     int_sincos_table[i] = sin (double (i / 256.0) * 2 * M_PI);
 362 }
 363
 364 inline double
 365 window_blackman_harris_92 (double x)
 366 {
 367   if (fabs (x) > 1)
 368     return 0;
 369
 370   const double a0 = 0.35875, a1 = 0.48829, a2 = 0.14128, a3 = 0.01168;
 371
 372   return a0 + a1 * cos (M_PI * x) + a2 * cos (2.0 * M_PI * x) + a3 * cos (3.0 * M_PI * x);
 373 }
 374
 375 #if defined (__i386__) && defined (__GNUC__)
 376 inline int
 377 sm_round_positive (double d)
 378 {
 379   return bse_dtoi (d);
 380 }
 381
 382 inline int
 383 sm_round_positive (float f)
 384 {
 385   return bse_ftoi (f);
 386 }
 387 #else
 388 inline int
 389 sm_round_positive (double d)
 390 {
 391   return int (d + 0.5);
 392 }
 393
 394 inline int
 395 sm_round_positive (float f)
 396 {
 397   return int (f + 0.5);
 398 }
 399 #endif
 400
 401 struct MathTables
 402 {
 403   static float idb2f_high[256];
 404   static float idb2f_low[256];
 405
 406   static float ifreq2f_high[256];
 407   static float ifreq2f_low[256];
 408 };
 409
 410 #define SM_IDB_CONST_M96 uint16_t ((512 - 96) * 64)
 411
 412 uint16_t sm_factor2idb (double factor);
 413 int      sm_factor2delta_idb (double factor);
 414 double   sm_idb2factor_slow (uint16_t idb);
 415
 416 void     sm_math_init();
 417
 418 uint16_t sm_freq2ifreq (double freq);
 419 double   sm_ifreq2freq_slow (uint16_t ifreq);
 420
 421 inline double
 422 sm_idb2factor (uint16_t idb)
 423 {
 424   return MathTables::idb2f_high[idb >> 8] * MathTables::idb2f_low[idb & 0xff];
 425 }
 426
 427 inline double
 428 sm_ifreq2freq (uint16_t ifreq)
 429 {
 430   return MathTables::ifreq2f_high[ifreq >> 8] * MathTables::ifreq2f_low[ifreq & 0xff];
 431 }
 432
 433
 434 } // namespace SpectMorph
 435
 436 #endif