cba4fbb1f6a862e0517922082f43a7503be2127e
[spectmorph.git] / 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 <stdint.h>
11 #ifdef __SSE__
12 #include <xmmintrin.h>
13 #endif
14
15 #include <algorithm>
16 #include <cmath>
17
18 namespace SpectMorph
19 {
20
21 /* Unfortunately, if we just write fabs(x) in our code, the return value type
22  * appears to be compiler/version dependant:
23  *  - if x is a double, the result is a double (just as in plain C)
24  *  - if x is a float
25  *    - some compilers return double (C style)
26  *    - some compilers return float (C++ style)
27  *
28  * This "using" should enforce C++ style behaviour for fabs(x) for all compilers,
29  * as long as we do using namespace SpectMorph (which we should always do).
30  */
31 using std::fabs;
32
33 /**
34  * \brief parameter structure for the various optimized vector sine functions
35  */
36 struct
37 VectorSinParams
38 {
39   double mix_freq;         //!< the mix freq (sampling rate) of the sin (and cos) wave to be created
40   double freq;             //!< the frequency of the sin (and cos) wave to be created
41   double phase;            //!< the start phase of the wave
42   double mag;              //!< the magnitude (amplitude) of the wave
43
44   enum {
45     NONE = -1,
46     ADD  = 1,              //!< add computed values to the values that are in the output array
47     REPLACE = 2            //!< replace values in the output array with computed values 
48   } mode;                  //!< whether to overwrite or add output
49
50   VectorSinParams() :
51     mix_freq (-1),
52     freq (-1),
53     phase (-100),
54     mag (-1),
55     mode (NONE)
56   {
57   }
58 };
59
60 template<class Iterator, int MODE>
61 inline void
62 internal_fast_vector_sin (const VectorSinParams& params, Iterator begin, Iterator end)
63 {
64   g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
65
66   const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
67   const double inc_re = cos (phase_inc);
68   const double inc_im = sin (phase_inc);
69   int n = 0;
70
71   double state_re;
72   double state_im;
73
74   sincos (params.phase, &state_im, &state_re);
75   state_re *= params.mag;
76   state_im *= params.mag;
77
78   for (Iterator x = begin; x != end; x++)
79     {
80       if (MODE == VectorSinParams::REPLACE)
81         *x = state_im;
82       else
83         *x += state_im;
84       if ((n++ & 255) == 255)
85         {
86           sincos (phase_inc * n + params.phase, &state_im, &state_re);
87           state_re *= params.mag;
88           state_im *= params.mag;
89         }
90       else
91         {
92           /*
93            * (state_re + i * state_im) * (inc_re + i * inc_im) =
94            *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
95            */
96           const double re = state_re * inc_re - state_im * inc_im;
97           const double im = state_re * inc_im + state_im * inc_re;
98           state_re = re;
99           state_im = im;
100         }
101     }
102 }
103
104 template<class Iterator, int MODE>
105 inline void
106 internal_fast_vector_sincos (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end, Iterator cos_begin)
107 {
108   g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
109
110   const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
111   const double inc_re = cos (phase_inc);
112   const double inc_im = sin (phase_inc);
113   int n = 0;
114
115   double state_re;
116   double state_im;
117
118   sincos (params.phase, &state_im, &state_re);
119   state_re *= params.mag;
120   state_im *= params.mag;
121
122   for (Iterator x = sin_begin, y = cos_begin; x != sin_end; x++, y++)
123     {
124       if (MODE == VectorSinParams::REPLACE)
125         {
126           *x = state_im;
127           *y = state_re;
128         }
129       else
130         {
131           *x += state_im;
132           *y += state_re;
133         }
134       if ((n++ & 255) == 255)
135         {
136           sincos (phase_inc * n + params.phase, &state_im, &state_re);
137           state_re *= params.mag;
138           state_im *= params.mag;
139         }
140       else
141         {
142           /*
143            * (state_re + i * state_im) * (inc_re + i * inc_im) =
144            *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
145            */
146           const double re = state_re * inc_re - state_im * inc_im;
147           const double im = state_re * inc_im + state_im * inc_re;
148           state_re = re;
149           state_im = im;
150         }
151     }
152 }
153
154 template<class Iterator>
155 inline void
156 fast_vector_sin (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end)
157 {
158   if (params.mode == VectorSinParams::ADD)
159     {
160       internal_fast_vector_sin<Iterator, VectorSinParams::ADD> (params, sin_begin, sin_end);
161     }
162   else if (params.mode == VectorSinParams::REPLACE)
163     {
164       internal_fast_vector_sin<Iterator, VectorSinParams::REPLACE> (params, sin_begin, sin_end);
165     }
166   else
167     {
168       g_assert_not_reached();
169     }
170 }
171
172 template<class Iterator>
173 inline void
174 fast_vector_sincos (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end, Iterator cos_begin)
175 {
176   if (params.mode == VectorSinParams::ADD)
177     {
178       internal_fast_vector_sincos<Iterator, VectorSinParams::ADD> (params, sin_begin, sin_end, cos_begin);
179     }
180   else if (params.mode == VectorSinParams::REPLACE)
181     {
182       internal_fast_vector_sincos<Iterator, VectorSinParams::REPLACE> (params, sin_begin, sin_end, cos_begin);
183     }
184   else
185     {
186       g_assert_not_reached();
187     }
188 }
189
190
191 /// @cond
192 /* see: http://ds9a.nl/gcc-simd/ */
193 union F4Vector
194 {
195   float f[4];
196 #ifdef __SSE__
197   __m128 v;   // vector of four single floats
198 #endif
199 };
200 /// @endcond
201
202 template<bool NEED_COS, int MODE>
203 inline void
204 internal_fast_vector_sincosf (const VectorSinParams& params, float *sin_begin, float *sin_end, float *cos_begin)
205 {
206 #ifdef __SSE__
207   g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
208
209   const int TABLE_SIZE = 32;
210
211   const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
212   const double inc_re16 = cos (phase_inc * TABLE_SIZE * 4);
213   const double inc_im16 = sin (phase_inc * TABLE_SIZE * 4);
214   int n = 0;
215
216   double state_re;
217   double state_im;
218
219   sincos (params.phase, &state_im, &state_re);
220   state_re *= params.mag;
221   state_im *= params.mag;
222
223   F4Vector incf_re[TABLE_SIZE];
224   F4Vector incf_im[TABLE_SIZE];
225
226   // compute tables using FPU
227   VectorSinParams table_params = params;
228   table_params.phase = 0;
229   table_params.mag = 1;
230   table_params.mode = VectorSinParams::REPLACE;
231   fast_vector_sincos (table_params, incf_im[0].f, incf_im[0].f + (TABLE_SIZE * 4), incf_re[0].f);
232
233   // inner loop using SSE instructions
234   int todo = sin_end - sin_begin;
235   while (todo >= 4 * TABLE_SIZE)
236     {
237       F4Vector sf_re;
238       F4Vector sf_im;
239       sf_re.f[0] = state_re;
240       sf_re.f[1] = state_re;
241       sf_re.f[2] = state_re;
242       sf_re.f[3] = state_re;
243       sf_im.f[0] = state_im;
244       sf_im.f[1] = state_im;
245       sf_im.f[2] = state_im;
246       sf_im.f[3] = state_im;
247
248       /*
249        * formula for complex multiplication:
250        *
251        * (state_re + i * state_im) * (inc_re + i * inc_im) =
252        *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
253        */
254       F4Vector *new_im = reinterpret_cast<F4Vector *> (sin_begin + n);
255       F4Vector *new_re = reinterpret_cast<F4Vector *> (cos_begin + n);
256       for (int k = 0; k < TABLE_SIZE; k++)
257         {
258           if (MODE == VectorSinParams::ADD)
259             {
260               if (NEED_COS)
261                 {
262                   new_re[k].v = _mm_add_ps (new_re[k].v, _mm_sub_ps (_mm_mul_ps (sf_re.v, incf_re[k].v),
263                                                                      _mm_mul_ps (sf_im.v, incf_im[k].v)));
264                 }
265               new_im[k].v = _mm_add_ps (new_im[k].v, _mm_add_ps (_mm_mul_ps (sf_re.v, incf_im[k].v),
266                                                      _mm_mul_ps (sf_im.v, incf_re[k].v)));
267             }
268           else
269             {
270               if (NEED_COS)
271                 {
272                   new_re[k].v = _mm_sub_ps (_mm_mul_ps (sf_re.v, incf_re[k].v),
273                                             _mm_mul_ps (sf_im.v, incf_im[k].v));
274                 }
275               new_im[k].v = _mm_add_ps (_mm_mul_ps (sf_re.v, incf_im[k].v),
276                                         _mm_mul_ps (sf_im.v, incf_re[k].v));
277             }
278         }
279
280       n += 4 * TABLE_SIZE;
281
282       /*
283        * (state_re + i * state_im) * (inc_re + i * inc_im) =
284        *   state_re * inc_re - state_im * inc_im + i * (state_re * inc_im + state_im * inc_re)
285        */
286       const double re = state_re * inc_re16 - state_im * inc_im16;
287       const double im = state_re * inc_im16 + state_im * inc_re16;
288       state_re = re;
289       state_im = im;
290
291       todo -= 4 * TABLE_SIZE;
292     }
293
294   // compute the remaining sin/cos values using the FPU
295   VectorSinParams rest_params = params;
296   rest_params.phase += n * phase_inc;
297   if (NEED_COS)
298     fast_vector_sincos (rest_params, sin_begin + n, sin_end, cos_begin + n);
299   else
300     fast_vector_sin (rest_params, sin_begin + n, sin_end);
301 #else
302   if (NEED_COS)
303     fast_vector_sincos (params, sin_begin, sin_end, cos_begin);
304   else
305     fast_vector_sin (params, sin_begin, sin_end);
306 #endif
307 }
308
309 inline void
310 fast_vector_sincosf (const VectorSinParams& params, float *sin_begin, float *sin_end, float *cos_begin)
311 {
312   if (params.mode == VectorSinParams::ADD)
313     {
314       internal_fast_vector_sincosf<true, VectorSinParams::ADD> (params, sin_begin, sin_end, cos_begin);
315     }
316   else if (params.mode == VectorSinParams::REPLACE)
317     {
318       internal_fast_vector_sincosf<true, VectorSinParams::REPLACE> (params, sin_begin, sin_end, cos_begin);
319     }
320   else
321     {
322       g_assert_not_reached();
323     }
324 }
325
326 inline void
327 fast_vector_sinf (const VectorSinParams& params, float *sin_begin, float *sin_end)
328 {
329   if (params.mode == VectorSinParams::ADD)
330     {
331       internal_fast_vector_sincosf<false, VectorSinParams::ADD> (params, sin_begin, sin_end, NULL);
332     }
333   else if (params.mode == VectorSinParams::REPLACE)
334     {
335       internal_fast_vector_sincosf<false, VectorSinParams::REPLACE> (params, sin_begin, sin_end, NULL);
336     }
337   else
338     {
339       g_assert_not_reached();
340     }
341 }
342
343 inline void
344 zero_float_block (size_t n_values, float *values)
345 {
346   memset (values, 0, n_values * sizeof (float));
347 }
348
349 inline float
350 int_sinf (guint8 i)
351 {
352   extern float *int_sincos_table;
353
354   return int_sincos_table[i];
355 }
356
357 inline float
358 int_cosf (guint8 i)
359 {
360   extern float *int_sincos_table;
361
362   i += 64;
363   return int_sincos_table[i];
364 }
365
366 inline void
367 int_sincos_init()
368 {
369   extern float *int_sincos_table;
370
371   int_sincos_table = (float *) malloc (sizeof (float) * 256);
372   for (int i = 0; i < 256; i++)
373     int_sincos_table[i] = sin (double (i / 256.0) * 2 * M_PI);
374 }
375
376 /* --- signal processing: windows --- */
377
378 inline double
379 window_cos (double x) /* von Hann window */
380 {
381   if (fabs (x) > 1)
382     return 0;
383   return 0.5 * cos (x * M_PI) + 0.5;
384 }
385
386 inline double
387 window_hamming (double x) /* sharp (rectangle) cutoffs at boundaries */
388 {
389   if (fabs (x) > 1)
390     return 0;
391
392   return 0.54 + 0.46 * cos (M_PI * x);
393 }
394
395 inline double
396 window_blackman (double x)
397 {
398   if (fabs (x) > 1)
399     return 0;
400   return 0.42 + 0.5 * cos (M_PI * x) + 0.08 * cos (2.0 * M_PI * x);
401 }
402
403 inline double
404 window_blackman_harris_92 (double x)
405 {
406   if (fabs (x) > 1)
407     return 0;
408
409   const double a0 = 0.35875, a1 = 0.48829, a2 = 0.14128, a3 = 0.01168;
410
411   return a0 + a1 * cos (M_PI * x) + a2 * cos (2.0 * M_PI * x) + a3 * cos (3.0 * M_PI * x);
412 }
413
414 /* --- decibel conversion --- */
415 double db_to_factor (double dB);
416 double db_from_factor (double factor, double min_dB);
417
418 #if defined (__i386__) && defined (__GNUC__)
419 static inline int G_GNUC_CONST
420 sm_ftoi (register float f)
421 {
422   int r;
423
424   __asm__ ("fistl %0"
425            : "=m" (r)
426            : "t" (f));
427   return r;
428 }
429 static inline int G_GNUC_CONST
430 sm_dtoi (register double f)
431 {
432   int r;
433
434   __asm__ ("fistl %0"
435            : "=m" (r)
436            : "t" (f));
437   return r;
438 }
439 inline int
440 sm_round_positive (double d)
441 {
442   return sm_dtoi (d);
443 }
444
445 inline int
446 sm_round_positive (float f)
447 {
448   return sm_ftoi (f);
449 }
450 #else
451 inline int
452 sm_round_positive (double d)
453 {
454   return int (d + 0.5);
455 }
456
457 inline int
458 sm_round_positive (float f)
459 {
460   return int (f + 0.5);
461 }
462 #endif
463
464 int sm_fpu_okround();
465
466 struct MathTables
467 {
468   static float idb2f_high[256];
469   static float idb2f_low[256];
470
471   static float ifreq2f_high[256];
472   static float ifreq2f_low[256];
473 };
474
475 #define SM_IDB_CONST_M96 uint16_t ((512 - 96) * 64)
476
477 int      sm_factor2delta_idb (double factor);
478 double   sm_idb2factor_slow (uint16_t idb);
479
480 void     sm_math_init();
481
482 uint16_t sm_freq2ifreq (double freq);
483 double   sm_ifreq2freq_slow (uint16_t ifreq);
484
485 inline double
486 sm_idb2factor (uint16_t idb)
487 {
488   return MathTables::idb2f_high[idb >> 8] * MathTables::idb2f_low[idb & 0xff];
489 }
490
491 inline double
492 sm_ifreq2freq (uint16_t ifreq)
493 {
494   return MathTables::ifreq2f_high[ifreq >> 8] * MathTables::ifreq2f_low[ifreq & 0xff];
495 }
496
497 inline uint16_t
498 sm_factor2idb (double factor)
499 {
500   /* 1e-25 is about the smallest factor we can properly represent as integer, as
501    *
502    *   20 * log10(1e-25) = 20 * -25 = -500 db
503    *
504    * so we map every factor that is smaller, like 0, to this value
505    */
506   const double db = 20 * log10 (std::max (factor, 1e-25));
507
508   return sm_round_positive (db * 64 + 512 * 64);
509 }
510
511 double sm_lowpass1_factor (double mix_freq, double freq);
512 double sm_xparam (double x, double slope);
513 double sm_xparam_inv (double x, double slope);
514
515 double sm_bessel_i0 (double x);
516
517 template<typename T>
518 inline const T&
519 sm_bound (const T& min_value, const T& value, const T& max_value)
520 {
521   return std::min (std::max (value, min_value), max_value);
522 }
523
524 } // namespace SpectMorph
525
526 #endif