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