plugins/libfourier/fourier.C

   1 #include <math.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4
   5 #include "clip.h"
   6 #include "fourier.h"
   7 #include "transportque.inc"
   8
   9 #define HALF_WINDOW (window_size / 2)
  10
  11
  12 // we need to do some trickery to get around of fftw thread unsafetyness
  13 fftw_plan_desc *FFT::fftw_plans = 0;
  14 Mutex FFT::plans_lock = Mutex();
  15
  16 FFT::FFT()
  17 {
  18 }
  19
  20 FFT::~FFT()
  21 {
  22 }
  23
  24 int FFT::do_fft(unsigned int samples,  // must be a power of 2
  25         int inverse,         // 0 = forward FFT, 1 = inverse
  26         double *real_in,     // array of input's real samples
  27         double *imag_in,     // array of input's imag samples
  28         double *real_out,    // array of output's reals
  29         double *imag_out)
  30 {
  31     unsigned int num_bits;    // Number of bits needed to store indices
  32     unsigned int i, j, k, n;
  33     unsigned int block_size, block_end;
  34
  35     double angle_numerator = 2.0 * M_PI;
  36     double tr, ti;     // temp real, temp imaginary
  37
  38     if(inverse)
  39         angle_numerator = -angle_numerator;
  40
  41     num_bits = samples_to_bits(samples);
  42
  43 // Do simultaneous data copy and bit-reversal ordering into outputs
  44
  45     for(i = 0; i < samples; i++)
  46     {
  47         j = reverse_bits(i, num_bits);
  48         real_out[j] = real_in[i];
  49         imag_out[j] = (imag_in == 0) ? 0.0 : imag_in[i];
  50     }
  51
  52 // Do the FFT itself
  53
  54     block_end = 1;
  55     double delta_angle;
  56     double sm2;
  57     double sm1;
  58     double cm2;
  59     double cm1;
  60     double w;
  61     double ar[3], ai[3];
  62     double temp;
  63     for(block_size = 2; block_size <= samples; block_size <<= 1)
  64     {
  65         delta_angle = angle_numerator / (double)block_size;
  66         sm2 = sin(-2 * delta_angle);
  67         sm1 = sin(-delta_angle);
  68         cm2 = cos(-2 * delta_angle);
  69         cm1 = cos(-delta_angle);
  70         w = 2 * cm1;
  71
  72         for(i = 0; i < samples; i += block_size)
  73         {
  74             ar[2] = cm2;
  75             ar[1] = cm1;
  76
  77             ai[2] = sm2;
  78             ai[1] = sm1;
  79
  80             for(j = i, n = 0; n < block_end; j++, n++)
  81             {
  82                 ar[0] = w * ar[1] - ar[2];
  83                 ar[2] = ar[1];
  84                 ar[1] = ar[0];
  85
  86                 ai[0] = w * ai[1] - ai[2];
  87                 ai[2] = ai[1];
  88                 ai[1] = ai[0];
  89
  90                 k = j + block_end;
  91                 tr = ar[0] * real_out[k] - ai[0] * imag_out[k];
  92                 ti = ar[0] * imag_out[k] + ai[0] * real_out[k];
  93
  94                 real_out[k] = real_out[j] - tr;
  95                 imag_out[k] = imag_out[j] - ti;
  96
  97                 real_out[j] += tr;
  98                 imag_out[j] += ti;
  99             }
 100         }
 101
 102         block_end = block_size;
 103     }
 104
 105 // Normalize if inverse transform
 106
 107     if(inverse)
 108     {
 109         double denom = (double)samples;
 110
 111         for (i = 0; i < samples; i++)
 112         {
 113             real_out[i] /= denom;
 114             imag_out[i] /= denom;
 115         }
 116     }
 117         return 0;
 118 }
 119
 120 int FFT::update_progress(int current_position)
 121 {
 122         return 0;
 123 }
 124
 125 unsigned int FFT::samples_to_bits(unsigned int samples)
 126 {
 127     unsigned int i;
 128
 129     for(i = 0; ; i++)
 130     {
 131         if(samples & (1 << i))
 132             return i;
 133     }
 134         return i;
 135 }
 136
 137 unsigned int FFT::reverse_bits(unsigned int index, unsigned int bits)
 138 {
 139     unsigned int i, rev;
 140
 141     for(i = rev = 0; i < bits; i++)
 142     {
 143         rev = (rev << 1) | (index & 1);
 144         index >>= 1;
 145     }
 146
 147     return rev;
 148 }
 149
 150 int FFT::symmetry(int size, double *freq_real, double *freq_imag)
 151 {
 152     int h = size / 2;
 153     for(int i = h + 1; i < size; i++)
 154     {
 155         freq_real[i] = freq_real[size - i];
 156         freq_imag[i] = -freq_imag[size - i];
 157     }
 158         return 0;
 159 }
 160
 161 // Create a proper fftw plan to be used later
 162 int FFT::ready_fftw(unsigned int samples)
 163 {
 164 // FFTW plan generation is not thread safe, so we have to take precausions
 165         FFT::plans_lock.lock();
 166         fftw_plan_desc *plan;
 167
 168         my_fftw_plan = 0;
 169
 170         for (plan = fftw_plans; plan; plan = plan->next)
 171                 if (plan->samples == samples)
 172                 {
 173                         my_fftw_plan = plan;
 174                         break;
 175                 }
 176
 177         if (!my_fftw_plan)
 178         {
 179                 fftw_complex *temp_data = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * samples);
 180                 my_fftw_plan = new fftw_plan_desc;   // we never discard this, since they are static
 181                 my_fftw_plan->samples = samples;
 182                 my_fftw_plan->plan_forward = fftw_plan_dft_1d(samples, temp_data, temp_data, FFTW_FORWARD, FFTW_ESTIMATE);
 183                 my_fftw_plan->plan_backward = fftw_plan_dft_1d(samples, temp_data, temp_data, FFTW_BACKWARD, FFTW_ESTIMATE);
 184                 // We will use this plan only in guru mode so we can now discard the temp_data
 185                 fftw_free(temp_data);
 186
 187                 // Put the plan into the linked list
 188                 my_fftw_plan->next = fftw_plans;
 189                 fftw_plans = my_fftw_plan;
 190         }
 191
 192         FFT::plans_lock.unlock();
 193         return 0;
 194 }
 195
 196 int FFT::do_fftw_inplace(unsigned int samples,
 197                 int inverse,
 198                 fftw_complex *data)
 199 {
 200         if (inverse == 0)
 201                 fftw_execute_dft(my_fftw_plan->plan_forward, data, data);
 202         else
 203                 fftw_execute_dft(my_fftw_plan->plan_backward, data, data);
 204 }
 205
 206
 207
 208
 209 CrossfadeFFT::CrossfadeFFT() : FFT()
 210 {
 211         reset();
 212         window_size = 4096;
 213 }
 214
 215 CrossfadeFFT::~CrossfadeFFT()
 216 {
 217         delete_fft();
 218 }
 219
 220 int CrossfadeFFT::reset()
 221 {
 222         input_buffer = 0;
 223         output_buffer = 0;
 224         freq_real = 0;
 225         freq_imag = 0;
 226         temp_real = 0;
 227         temp_imag = 0;
 228         first_window = 1;
 229 // samples in input_buffer and output_buffer
 230         input_size = 0;
 231         output_size = 0;
 232         input_allocation = 0;
 233         output_allocation = 0;
 234         output_sample = 0;
 235         input_sample = 0;
 236         samples_ready = 0;
 237         oversample = 0;
 238         pre_window = 0;
 239         post_window = 0;
 240         fftw_data = 0;
 241         return 0;
 242 }
 243
 244 int CrossfadeFFT::delete_fft()
 245 {
 246         if(input_buffer) delete [] input_buffer;
 247         if(output_buffer) delete [] output_buffer;
 248         if(freq_real) delete [] freq_real;
 249         if(freq_imag) delete [] freq_imag;
 250         if(temp_real) delete [] temp_real;
 251         if(temp_imag) delete [] temp_imag;
 252         if(pre_window) delete [] pre_window;
 253         if(post_window) delete [] post_window;
 254         if(fftw_data) fftw_free(fftw_data);
 255         reset();
 256         return 0;
 257 }
 258
 259 int CrossfadeFFT::fix_window_size()
 260 {
 261 // fix the window size
 262 // window size must be a power of 2
 263         int new_size = 16;
 264         while(new_size < window_size) new_size *= 2;
 265         window_size = MIN(131072, window_size);
 266         window_size = new_size;
 267         return 0;
 268 }
 269
 270 int CrossfadeFFT::initialize(int window_size)
 271 {
 272         this->window_size = window_size;
 273         first_window = 1;
 274         reconfigure();
 275         return 0;
 276 }
 277
 278 long CrossfadeFFT::get_delay()
 279 {
 280         return window_size + HALF_WINDOW;
 281 }
 282
 283 int CrossfadeFFT::reconfigure()
 284 {
 285         delete_fft();
 286         fix_window_size();
 287
 288
 289
 290         return 0;
 291 }
 292
 293
 294
 295 int CrossfadeFFT::process_buffer(int64_t output_sample,
 296         long size,
 297         double *output_ptr,
 298         int direction)
 299 {
 300         int result = 0;
 301         int step = (direction == PLAY_FORWARD) ? 1 : -1;
 302
 303         if(output_sample != this->output_sample || first_window)
 304         {
 305                 output_size = 0;
 306                 input_size = 0;
 307                 first_window = 1;
 308                 this->output_sample = output_sample;
 309                 this->input_sample = output_sample;
 310         }
 311
 312 // Fill output buffer half a window at a time until size samples are available
 313         while(output_size < size)
 314         {
 315                 if(!input_buffer) input_buffer = new double[window_size];
 316                 if(!freq_real) freq_real = new double[window_size];
 317                 if(!freq_imag) freq_imag = new double[window_size];
 318                 if(!temp_real) temp_real = new double[window_size];
 319                 if(!temp_imag) temp_imag = new double[window_size];
 320
 321 // Fill enough input to make a window starting at output_sample
 322                 if(first_window)
 323                         result = read_samples(this->input_sample,
 324                                 window_size,
 325                                 input_buffer);
 326                 else
 327                         result = read_samples(this->input_sample + step * HALF_WINDOW,
 328                                 HALF_WINDOW,
 329                                 input_buffer + HALF_WINDOW);
 330
 331                 input_size = window_size;
 332
 333                 if(!result)
 334                         do_fft(window_size,   // must be a power of 2
 335                         0,                // 0 = forward FFT, 1 = inverse
 336                         input_buffer,     // array of input's real samples
 337                         0,                // array of input's imag samples
 338                         freq_real,        // array of output's reals
 339                         freq_imag);
 340                 if(!result)
 341                         result = signal_process();
 342                 if(!result)
 343                         do_fft(window_size,  // must be a power of 2
 344                         1,               // 0 = forward FFT, 1 = inverse
 345                         freq_real,       // array of input's real samples
 346                         freq_imag,       // array of input's imag samples
 347                         temp_real,       // array of output's reals
 348                         temp_imag);      // array of output's imaginaries
 349
 350 // Allocate output buffer
 351                 int new_allocation = output_size + window_size;
 352                 if(new_allocation > output_allocation)
 353                 {
 354                         double *new_output = new double[new_allocation];
 355                         if(output_buffer)
 356                         {
 357                                 memcpy(new_output,
 358                                         output_buffer,
 359                                         sizeof(double) * (output_size + HALF_WINDOW));
 360                                 delete [] output_buffer;
 361                         }
 362                         output_buffer = new_output;
 363                         output_allocation = new_allocation;
 364                 }
 365
 366 // Overlay processed buffer
 367                 if(first_window)
 368                 {
 369                         memcpy(output_buffer + output_size,
 370                                 temp_real,
 371                                 sizeof(double) * window_size);
 372                         first_window = 0;
 373                 }
 374                 else
 375                 {
 376                         for(int i = 0, j = output_size; i < HALF_WINDOW; i++, j++)
 377                         {
 378                                 double src_level = (double)i / HALF_WINDOW;
 379                                 double dst_level = (double)(HALF_WINDOW - i) / HALF_WINDOW;
 380                                 output_buffer[j] = output_buffer[j] * dst_level +
 381                                         temp_real[i] * src_level;
 382                         }
 383
 384                         memcpy(output_buffer + output_size + HALF_WINDOW,
 385                                 temp_real + HALF_WINDOW,
 386                                 sizeof(double) * HALF_WINDOW);
 387                 }
 388
 389                 output_size += HALF_WINDOW;
 390
 391 // Shift input buffer
 392                 for(int i = window_size - HALF_WINDOW, j = 0;
 393                         i < input_size;
 394                         i++, j++)
 395                 {
 396                         input_buffer[j] = input_buffer[i];
 397                 }
 398                 input_size = HALF_WINDOW;
 399                 this->input_sample += step * HALF_WINDOW;
 400         }
 401
 402
 403
 404 // Transfer output buffer
 405         if(output_ptr)
 406         {
 407                 memcpy(output_ptr, output_buffer, sizeof(double) * size);
 408         }
 409         for(int i = 0, j = size; j < output_size + HALF_WINDOW; i++, j++)
 410                 output_buffer[i] = output_buffer[j];
 411         this->output_sample += step * size;
 412         this->output_size -= size;
 413
 414         return 0;
 415 }
 416
 417 void CrossfadeFFT::set_oversample(int oversample)
 418 {
 419 // Only powers of two can be used for oversample
 420         int oversample_fix = 2;
 421         while(oversample_fix < oversample) oversample_fix *= 2;
 422         this->oversample = oversample = oversample_fix;
 423
 424 // Precalculate the pre-envelope hanning window
 425         pre_window = new double[window_size];
 426         for (int i = 0; i< window_size; i++)
 427                 pre_window[i] = 0.5 - 0.5 *cos(2 * M_PI * i / window_size);
 428
 429 // Precalculate the post-envelope hanning window also, we could have triangle here also
 430         post_window = new double[window_size];
 431 /*      for (int i = 0; i< window_size/2; i++)
 432                 post_window[i] = 1.0 * i / (window_size/2) / oversample * 2;
 433         for (int i = window_size/2; i< window_size; i++)
 434                 post_window[i] = 1.0 * (window_size - i) / (window_size/2) / oversample * 2;
 435  */
 436         for (int i = 0; i< window_size; i++)
 437                 post_window[i] = (0.5 - 0.5 *cos(2 * M_PI * i / window_size)) * 6/ oversample / window_size;
 438
 439         ready_fftw(window_size);
 440
 441 }
 442
 443 void smbFft(double *fftBuffer, long fftFrameSize, long sign);
 444
 445
 446
 447
 448 int CrossfadeFFT::process_buffer_oversample(int64_t output_sample,
 449         long size,
 450         double *output_ptr,
 451         int direction)
 452 {
 453         if (oversample <= 0)
 454         {
 455                 printf("set_oversample() has to be called to use process_buffer_oversample\n");
 456                 return 1;
 457         }
 458         int result = 0;
 459         int step = (direction == PLAY_FORWARD) ? 1 : -1;
 460
 461         int overlap_size = window_size / oversample;
 462         int total_size;
 463         int start_skip;
 464
 465         if (!output_ptr)
 466         {
 467                 printf("ERROR, no output pointer!\n");
 468                 return 1;
 469         }
 470         if(output_sample != this->output_sample || first_window)
 471         {
 472                 input_size = 0;
 473                 first_window = 1;
 474                 this->output_sample = output_sample;
 475                 samples_ready = 0;
 476                 start_skip = window_size - overlap_size;
 477                 total_size = size + start_skip;
 478                 // signal_process() will always be able to know which window it has by looking at input_sample
 479                 this->input_sample = output_sample - step * start_skip;
 480                 if (step == -1) this->input_sample += overlap_size;
 481         } else
 482         {
 483                 start_skip = 0;
 484                 total_size = size;
 485                 first_window = 0;
 486         }
 487
 488 // Find out how big output buffer we will need, take overlapping into account
 489         int new_allocation = total_size + window_size;
 490         if(new_allocation > output_allocation)
 491         {
 492                 double *new_output = new double[new_allocation];
 493                 if(output_buffer)
 494                 {
 495                         memcpy(new_output,
 496                                 output_buffer,
 497                                 sizeof(double) * (samples_ready + window_size - overlap_size));
 498                         delete [] output_buffer;
 499
 500                 }
 501                 output_buffer = new_output;
 502                 output_allocation = new_allocation;
 503         }
 504 // Fill output buffer by overlap_size at a time until size samples are available
 505         while(samples_ready < total_size)
 506         {
 507                 if(!input_buffer) input_buffer = new double[window_size];
 508                 if(!fftw_data) fftw_data = (fftw_complex *)fftw_malloc(window_size * sizeof(fftw_complex));
 509
 510 // Fill enough input to make a window starting at output_sample
 511                 int64_t read_start;
 512                 int write_pos;
 513                 int read_len;
 514
 515                 if(first_window)
 516                 {
 517                         if (step == 1)
 518                                 read_start = this->input_sample;
 519                         else
 520                                 read_start = this->input_sample - window_size;
 521                         write_pos = 0;
 522                         read_len = window_size;
 523                 } else
 524                 {
 525                         if (step == 1)
 526                         {
 527                                 read_start = this->input_sample + window_size - overlap_size;
 528                                 write_pos = window_size - overlap_size;
 529                         } else
 530                         {
 531                                 read_start = this->input_sample - window_size;
 532                                 write_pos = 0;
 533                         }
 534                         read_len = overlap_size;
 535                 }
 536
 537                 if (read_start + read_len * step< 0)
 538                 {
 539 // completely outside the track
 540                         memset (input_buffer + write_pos, 0, read_len * sizeof(double));
 541                         result = 1;
 542                 } else
 543                 if (read_start < 0)
 544                 {
 545 // special case for reading before the track - in general it would be sensible that this behaviour is done by read_samples()
 546                         memset (input_buffer + write_pos, 0, - read_start * sizeof(double));
 547                         result = read_samples(0,
 548                                 read_start + read_len,
 549                                 input_buffer - read_start + write_pos);
 550                 } else
 551                 {
 552 //printf("Readstart: %lli, read len: %i, write pos: %i\n", read_start, read_len, write_pos);
 553                         result = read_samples(read_start,
 554                                 read_len,
 555                                 input_buffer + write_pos);
 556                 }
 557
 558
 559 // apply Hanning window to input samples
 560                 for (int i = 0; i< window_size; i++)
 561                 {
 562                         fftw_data[i][0] = input_buffer[i] * pre_window[i];
 563                         fftw_data[i][1] = 0;
 564                 }
 565
 566
 567                 if(!result)
 568                         do_fftw_inplace(window_size, 0, fftw_data);
 569                 if(!result)
 570                         result = signal_process_oversample(first_window);
 571                 if(!result)
 572                         do_fftw_inplace(window_size, 1, fftw_data);
 573
 574 // Overlay over existing output - overlap processing
 575                 if (step == 1)
 576                 {
 577                         for (int i = 0; i < window_size - overlap_size; i++)
 578                                 output_buffer[i + samples_ready] += fftw_data[i][0] * post_window[i];
 579                         for (int i = window_size - overlap_size; i < window_size; i++)
 580                                 output_buffer[i + samples_ready] = fftw_data[i][0] * post_window[i];
 581                 } else
 582                 {
 583                         int offset = output_allocation - samples_ready - window_size;
 584                         for (int i = 0; i < overlap_size; i++)
 585                                 output_buffer[i + offset] = fftw_data[i][0] * post_window[i];
 586                         for (int i = overlap_size; i < window_size; i++)
 587                                 output_buffer[i + offset] += fftw_data[i][0] * post_window[i];
 588                 }
 589
 590
 591 // Shift input buffer
 592                 if (step == 1)
 593                         memmove(input_buffer, input_buffer + overlap_size, (window_size - overlap_size) * sizeof(double));
 594                 else
 595                         memmove(input_buffer + overlap_size, input_buffer, (window_size - overlap_size) * sizeof(double));
 596
 597                 this->input_sample += step * overlap_size;
 598
 599                 samples_ready += overlap_size;
 600                 first_window = 0;
 601         }
 602
 603         if (step == 1)
 604         {
 605                 memcpy(output_ptr, output_buffer + start_skip , size * sizeof(double));
 606                 samples_ready -= total_size;
 607
 608                 memmove(output_buffer,
 609                         output_buffer + total_size,
 610                         (samples_ready + window_size - overlap_size) * sizeof(double));
 611                 this->output_sample += size;
 612
 613         } else
 614         {
 615                 memcpy(output_ptr, output_buffer + output_allocation - total_size , size * sizeof(double));
 616                 samples_ready -= total_size;
 617
 618                 memmove(output_buffer + output_allocation - (samples_ready + window_size - overlap_size),
 619                         output_buffer + output_allocation - (samples_ready + window_size - overlap_size) - total_size,
 620                         (samples_ready + window_size - overlap_size) * sizeof(double));
 621
 622                 this->output_sample -= size;
 623         }
 624
 625
 626         return 0;
 627 }
 628
 629
 630 int CrossfadeFFT::read_samples(int64_t output_sample,
 631                 int samples,
 632                 double *buffer)
 633 {
 634         return 1;
 635 }
 636
 637 int CrossfadeFFT::signal_process()
 638 {
 639         return 0;
 640 }
 641
 642 int CrossfadeFFT::signal_process_oversample(int reset)
 643 {
 644         return 0;
 645 }