00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef SPECTMORPH_MATH_HH
00019 #define SPECTMORPH_MATH_HH
00020
00021 #include <math.h>
00022 #include <glib.h>
00023 #include <string.h>
00024 #include <stdlib.h>
00025 #include <bse/bseieee754.h>
00026 #ifdef __SSE__
00027 #include <xmmintrin.h>
00028 #endif
00029
00030 namespace SpectMorph
00031 {
00032
00036 struct
00037 VectorSinParams
00038 {
00039 double mix_freq;
00040 double freq;
00041 double phase;
00042 double mag;
00043
00044 enum {
00045 NONE = -1,
00046 ADD = 1,
00047 REPLACE = 2
00048 } mode;
00049
00050 VectorSinParams() :
00051 mix_freq (-1),
00052 freq (-1),
00053 phase (-100),
00054 mag (-1),
00055 mode (NONE)
00056 {
00057 }
00058 };
00059
00060 template<class Iterator, int MODE>
00061 inline void
00062 internal_fast_vector_sin (const VectorSinParams& params, Iterator begin, Iterator end)
00063 {
00064 g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
00065
00066 const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
00067 const double inc_re = cos (phase_inc);
00068 const double inc_im = sin (phase_inc);
00069 int n = 0;
00070
00071 double state_re;
00072 double state_im;
00073
00074 sincos (params.phase, &state_im, &state_re);
00075 state_re *= params.mag;
00076 state_im *= params.mag;
00077
00078 for (Iterator x = begin; x != end; x++)
00079 {
00080 if (MODE == VectorSinParams::REPLACE)
00081 *x = state_im;
00082 else
00083 *x += state_im;
00084 if ((n++ & 255) == 255)
00085 {
00086 sincos (phase_inc * n + params.phase, &state_im, &state_re);
00087 state_re *= params.mag;
00088 state_im *= params.mag;
00089 }
00090 else
00091 {
00092
00093
00094
00095
00096 const double re = state_re * inc_re - state_im * inc_im;
00097 const double im = state_re * inc_im + state_im * inc_re;
00098 state_re = re;
00099 state_im = im;
00100 }
00101 }
00102 }
00103
00104 template<class Iterator, int MODE>
00105 inline void
00106 internal_fast_vector_sincos (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end, Iterator cos_begin)
00107 {
00108 g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
00109
00110 const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
00111 const double inc_re = cos (phase_inc);
00112 const double inc_im = sin (phase_inc);
00113 int n = 0;
00114
00115 double state_re;
00116 double state_im;
00117
00118 sincos (params.phase, &state_im, &state_re);
00119 state_re *= params.mag;
00120 state_im *= params.mag;
00121
00122 for (Iterator x = sin_begin, y = cos_begin; x != sin_end; x++, y++)
00123 {
00124 if (MODE == VectorSinParams::REPLACE)
00125 {
00126 *x = state_im;
00127 *y = state_re;
00128 }
00129 else
00130 {
00131 *x += state_im;
00132 *y += state_re;
00133 }
00134 if ((n++ & 255) == 255)
00135 {
00136 sincos (phase_inc * n + params.phase, &state_im, &state_re);
00137 state_re *= params.mag;
00138 state_im *= params.mag;
00139 }
00140 else
00141 {
00142
00143
00144
00145
00146 const double re = state_re * inc_re - state_im * inc_im;
00147 const double im = state_re * inc_im + state_im * inc_re;
00148 state_re = re;
00149 state_im = im;
00150 }
00151 }
00152 }
00153
00154 template<class Iterator>
00155 inline void
00156 fast_vector_sin (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end)
00157 {
00158 if (params.mode == VectorSinParams::ADD)
00159 {
00160 internal_fast_vector_sin<Iterator, VectorSinParams::ADD> (params, sin_begin, sin_end);
00161 }
00162 else if (params.mode == VectorSinParams::REPLACE)
00163 {
00164 internal_fast_vector_sin<Iterator, VectorSinParams::REPLACE> (params, sin_begin, sin_end);
00165 }
00166 else
00167 {
00168 g_assert_not_reached();
00169 }
00170 }
00171
00172 template<class Iterator>
00173 inline void
00174 fast_vector_sincos (const VectorSinParams& params, Iterator sin_begin, Iterator sin_end, Iterator cos_begin)
00175 {
00176 if (params.mode == VectorSinParams::ADD)
00177 {
00178 internal_fast_vector_sincos<Iterator, VectorSinParams::ADD> (params, sin_begin, sin_end, cos_begin);
00179 }
00180 else if (params.mode == VectorSinParams::REPLACE)
00181 {
00182 internal_fast_vector_sincos<Iterator, VectorSinParams::REPLACE> (params, sin_begin, sin_end, cos_begin);
00183 }
00184 else
00185 {
00186 g_assert_not_reached();
00187 }
00188 }
00189
00190
00192
00193 union F4Vector
00194 {
00195 float f[4];
00196 #ifdef __SSE__
00197 __m128 v;
00198 #endif
00199 };
00201
00202 template<bool NEED_COS, int MODE>
00203 inline void
00204 internal_fast_vector_sincosf (const VectorSinParams& params, float *sin_begin, float *sin_end, float *cos_begin)
00205 {
00206 #ifdef __SSE__
00207 g_return_if_fail (params.mix_freq > 0 && params.freq > 0 && params.phase > -99 && params.mag > 0);
00208
00209 const int TABLE_SIZE = 32;
00210
00211 const double phase_inc = params.freq / params.mix_freq * 2 * M_PI;
00212 const double inc_re16 = cos (phase_inc * TABLE_SIZE * 4);
00213 const double inc_im16 = sin (phase_inc * TABLE_SIZE * 4);
00214 int n = 0;
00215
00216 double state_re;
00217 double state_im;
00218
00219 sincos (params.phase, &state_im, &state_re);
00220 state_re *= params.mag;
00221 state_im *= params.mag;
00222
00223 F4Vector incf_re[TABLE_SIZE];
00224 F4Vector incf_im[TABLE_SIZE];
00225
00226
00227 VectorSinParams table_params = params;
00228 table_params.phase = 0;
00229 table_params.mag = 1;
00230 table_params.mode = VectorSinParams::REPLACE;
00231 fast_vector_sincos (table_params, incf_im[0].f, incf_im[0].f + (TABLE_SIZE * 4), incf_re[0].f);
00232
00233
00234 int todo = sin_end - sin_begin;
00235 while (todo >= 4 * TABLE_SIZE)
00236 {
00237 F4Vector sf_re;
00238 F4Vector sf_im;
00239 sf_re.f[0] = state_re;
00240 sf_re.f[1] = state_re;
00241 sf_re.f[2] = state_re;
00242 sf_re.f[3] = state_re;
00243 sf_im.f[0] = state_im;
00244 sf_im.f[1] = state_im;
00245 sf_im.f[2] = state_im;
00246 sf_im.f[3] = state_im;
00247
00248
00249
00250
00251
00252
00253
00254 F4Vector *new_im = reinterpret_cast<F4Vector *> (sin_begin + n);
00255 F4Vector *new_re = reinterpret_cast<F4Vector *> (cos_begin + n);
00256 for (int k = 0; k < TABLE_SIZE; k++)
00257 {
00258 if (MODE == VectorSinParams::ADD)
00259 {
00260 if (NEED_COS)
00261 {
00262 new_re[k].v = _mm_add_ps (new_re[k].v, _mm_sub_ps (_mm_mul_ps (sf_re.v, incf_re[k].v),
00263 _mm_mul_ps (sf_im.v, incf_im[k].v)));
00264 }
00265 new_im[k].v = _mm_add_ps (new_im[k].v, _mm_add_ps (_mm_mul_ps (sf_re.v, incf_im[k].v),
00266 _mm_mul_ps (sf_im.v, incf_re[k].v)));
00267 }
00268 else
00269 {
00270 if (NEED_COS)
00271 {
00272 new_re[k].v = _mm_sub_ps (_mm_mul_ps (sf_re.v, incf_re[k].v),
00273 _mm_mul_ps (sf_im.v, incf_im[k].v));
00274 }
00275 new_im[k].v = _mm_add_ps (_mm_mul_ps (sf_re.v, incf_im[k].v),
00276 _mm_mul_ps (sf_im.v, incf_re[k].v));
00277 }
00278 }
00279
00280 n += 4 * TABLE_SIZE;
00281
00282
00283
00284
00285
00286 const double re = state_re * inc_re16 - state_im * inc_im16;
00287 const double im = state_re * inc_im16 + state_im * inc_re16;
00288 state_re = re;
00289 state_im = im;
00290
00291 todo -= 4 * TABLE_SIZE;
00292 }
00293
00294
00295 VectorSinParams rest_params = params;
00296 rest_params.phase += n * phase_inc;
00297 if (NEED_COS)
00298 fast_vector_sincos (rest_params, sin_begin + n, sin_end, cos_begin + n);
00299 else
00300 fast_vector_sin (rest_params, sin_begin + n, sin_end);
00301 #else
00302 if (NEED_COS)
00303 fast_vector_sincos (params, sin_begin, sin_end, cos_begin);
00304 else
00305 fast_vector_sin (params, sin_begin, sin_end);
00306 #endif
00307 }
00308
00309 inline void
00310 fast_vector_sincosf (const VectorSinParams& params, float *sin_begin, float *sin_end, float *cos_begin)
00311 {
00312 if (params.mode == VectorSinParams::ADD)
00313 {
00314 internal_fast_vector_sincosf<true, VectorSinParams::ADD> (params, sin_begin, sin_end, cos_begin);
00315 }
00316 else if (params.mode == VectorSinParams::REPLACE)
00317 {
00318 internal_fast_vector_sincosf<true, VectorSinParams::REPLACE> (params, sin_begin, sin_end, cos_begin);
00319 }
00320 else
00321 {
00322 g_assert_not_reached();
00323 }
00324 }
00325
00326 inline void
00327 fast_vector_sinf (const VectorSinParams& params, float *sin_begin, float *sin_end)
00328 {
00329 if (params.mode == VectorSinParams::ADD)
00330 {
00331 internal_fast_vector_sincosf<false, VectorSinParams::ADD> (params, sin_begin, sin_end, NULL);
00332 }
00333 else if (params.mode == VectorSinParams::REPLACE)
00334 {
00335 internal_fast_vector_sincosf<false, VectorSinParams::REPLACE> (params, sin_begin, sin_end, NULL);
00336 }
00337 else
00338 {
00339 g_assert_not_reached();
00340 }
00341 }
00342
00343 inline void
00344 zero_float_block (size_t n_values, float *values)
00345 {
00346 memset (values, 0, n_values * sizeof (float));
00347 }
00348
00349 inline void
00350 int_sincos (guint8 i, double *si, double *ci)
00351 {
00352 extern float *int_sincos_table;
00353
00354 *si = int_sincos_table[i];
00355 i += 64;
00356 *ci = int_sincos_table[i];
00357 }
00358
00359 inline void
00360 int_sincosf (guint8 i, float *si, float *ci)
00361 {
00362 extern float *int_sincos_table;
00363
00364 *si = int_sincos_table[i];
00365 i += 64;
00366 *ci = int_sincos_table[i];
00367 }
00368
00369 inline void
00370 int_sincos_init()
00371 {
00372 extern float *int_sincos_table;
00373
00374 int_sincos_table = (float *) malloc (sizeof (float) * 256);
00375 for (int i = 0; i < 256; i++)
00376 int_sincos_table[i] = sin (double (i / 256.0) * 2 * M_PI);
00377 }
00378
00379 inline double
00380 window_blackman_harris_92 (double x)
00381 {
00382 if (fabs (x) > 1)
00383 return 0;
00384
00385 const double a0 = 0.35875, a1 = 0.48829, a2 = 0.14128, a3 = 0.01168;
00386
00387 return a0 + a1 * cos (M_PI * x) + a2 * cos (2.0 * M_PI * x) + a3 * cos (3.0 * M_PI * x);
00388 }
00389
00390 #if defined (__i386__) && defined (__GNUC__)
00391 inline int
00392 sm_round_positive (double d)
00393 {
00394 return bse_dtoi (d);
00395 }
00396
00397 inline int
00398 sm_round_positive (float f)
00399 {
00400 return bse_ftoi (f);
00401 }
00402 #else
00403 inline int
00404 sm_round_positive (double d)
00405 {
00406 return int (d + 0.5);
00407 }
00408
00409 inline int
00410 sm_round_positive (float f)
00411 {
00412 return int (f + 0.5);
00413 }
00414 #endif
00415
00416 }
00417
00418 #endif