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