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