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
sm_sincos(double x,double * s,double * c)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
VectorSinParamsSpectMorph::VectorSinParams62 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
internal_fast_vector_sin(const VectorSinParams & params,Iterator begin,Iterator end)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
internal_fast_vector_sincos(const VectorSinParams & params,Iterator sin_begin,Iterator sin_end,Iterator cos_begin)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
fast_vector_sin(const VectorSinParams & params,Iterator sin_begin,Iterator sin_end)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
fast_vector_sincos(const VectorSinParams & params,Iterator sin_begin,Iterator sin_end,Iterator cos_begin)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
internal_fast_vector_sincosf(const VectorSinParams & params,float * sin_begin,float * sin_end,float * cos_begin)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
fast_vector_sincosf(const VectorSinParams & params,float * sin_begin,float * sin_end,float * cos_begin)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
fast_vector_sinf(const VectorSinParams & params,float * sin_begin,float * sin_end)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
zero_float_block(size_t n_values,float * values)356 zero_float_block (size_t n_values, float *values)
357 {
358 memset (values, 0, n_values * sizeof (float));
359 }
360
361 inline float
int_sinf(guint8 i)362 int_sinf (guint8 i)
363 {
364 extern float *int_sincos_table;
365
366 return int_sincos_table[i];
367 }
368
369 inline float
int_cosf(guint8 i)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
int_sincos_init()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
window_cos(double x)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
window_hamming(double x)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
window_blackman(double x)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
window_blackman_harris_92(double x)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
sm_ftoi(register float f)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
sm_dtoi(register double f)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
sm_round_positive(double d)452 sm_round_positive (double d)
453 {
454 return sm_dtoi (d);
455 }
456
457 inline int
sm_round_positive(float f)458 sm_round_positive (float f)
459 {
460 return sm_ftoi (f);
461 }
462 #else
463 inline int
sm_round_positive(double d)464 sm_round_positive (double d)
465 {
466 return int (d + 0.5);
467 }
468
469 inline int
sm_round_positive(float f)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
sm_idb2factor(uint16_t idb)498 sm_idb2factor (uint16_t idb)
499 {
500 return MathTables::idb2f_high[idb >> 8] * MathTables::idb2f_low[idb & 0xff];
501 }
502
503 inline double
sm_ifreq2freq(uint16_t ifreq)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
sm_factor2idb(double factor)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&
sm_bound(const T & min_value,const T & value,const T & max_value)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
sm_atof(const char * nptr)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
547