f4ca7ed2864a42c517d1a5b2712e859e21cbd3cb
[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 <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