sound/weapons/loopfinder/findloop.c

   1 #include <stdio.h>
   2 #include <complex.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <math.h>
   6 #include <fftw3.h>
   7 #include <unistd.h>
   8 #include <err.h>
   9 #include <assert.h>
  10 #include <sndfile.h>
  11 #include <sys/types.h>
  12
  13 #define MIN(a,b) ((a)<(b) ? (a) : (b))
  14 #define MAX(a,b) ((a)>(b) ? (a) : (b))
  15
  16 void doFourier(double *input, int channels, sf_count_t len, fftw_complex *output)
  17 {
  18         static sf_count_t len0;
  19         static int channels0;
  20         static double *windowedData;
  21         static fftw_plan planlos;
  22         static int planned = 0;
  23         if(!planned)
  24         {
  25                 len0 = len;
  26                 channels0 = channels;
  27                 windowedData = fftw_malloc(sizeof(double) * channels * len);
  28
  29                 int l = len;
  30                 fprintf(stderr, "Planning...\n");
  31                 planlos = fftw_plan_many_dft_r2c(1, &l, channels,
  32                         windowedData, NULL, channels, 1,
  33                         output, NULL, channels, 1,
  34                         FFTW_MEASURE | FFTW_PRESERVE_INPUT | FFTW_UNALIGNED);
  35                 planned = 1;
  36         }
  37         assert(len0 == len);
  38         assert(channels0 == channels);
  39
  40         sf_count_t nmax = channels * len;
  41         sf_count_t i;
  42         for(i = 0; i < nmax; ++i)
  43         {
  44                 double ang = (i/channels) * 2 * M_PI / (len - 1);
  45                 windowedData[i] = input[i] * (
  46                         0.42
  47                         - 0.5 * cos(ang)
  48                         + 0.08 * cos(2 * ang)
  49                 ); // Blackman
  50         }
  51
  52         fftw_execute_dft_r2c(planlos, windowedData, output);
  53 }
  54
  55 double vectorDot(fftw_complex *v1, fftw_complex *v2, size_t length)
  56 {
  57         size_t i;
  58         double sum = 0;
  59         for(i = 0; i < length; ++i)
  60                 sum += cabs(v1[i]) * cabs(v2[i]);
  61         return sum;
  62 }
  63
  64 sf_count_t findMaximumSingle(double (*func) (sf_count_t), sf_count_t x0, sf_count_t x1, sf_count_t step)
  65 {
  66         sf_count_t bestpos = x1;
  67         double best = func(x1);
  68
  69         sf_count_t i;
  70
  71         for(i = x0; i < x1; i += step)
  72         {
  73                 double cur = func(i);
  74                 if(cur > best)
  75                 {
  76                         bestpos = i;
  77                         best = cur;
  78                 }
  79         }
  80
  81         return bestpos;
  82 }
  83
  84 sf_count_t findMaximum(double (*func) (sf_count_t), sf_count_t x0, sf_count_t xg, sf_count_t xg2, sf_count_t x1)
  85 {
  86         sf_count_t xg0, xg20;
  87
  88         xg0 = xg = MAX(x0, MIN(xg, x1));
  89         xg20 = xg2 = MAX(x0, MIN(xg2, x1));
  90
  91         fprintf(stderr, "min/max: %d %d\n", (int)xg, (int)xg2);
  92
  93         for(;;)
  94         {
  95                 sf_count_t size = xg2 - xg;
  96                 if(size == 0)
  97                         break;
  98                 //fprintf(stderr, "round:\n");
  99                 sf_count_t bestguess = findMaximumSingle(func, xg, xg2, size / 32 + 1);
 100                 xg = MAX(xg0, bestguess - size / 3);
 101                 xg2 = MIN(bestguess + size / 3, xg20);
 102         }
 103
 104         fprintf(stderr, "guessed: %d\n", (int)xg);
 105
 106         if(xg - xg0 < (xg20 - xg0) / 64)
 107                 fprintf(stderr, "warning: best match very close to left margin, maybe decrease the guess?\n");
 108
 109         if(xg20 - xg < (xg20 - xg0) / 64)
 110                 fprintf(stderr, "warning: best match very close to right margin, maybe increase the guess?\n");
 111
 112         return xg;
 113 }
 114
 115 int main(int argc, char **argv)
 116 {
 117         int chans;
 118         SF_INFO infile_info;
 119         memset(&infile_info, 0, sizeof(infile_info));
 120         infile_info.format = 0;
 121         SNDFILE *infile = sf_open(argv[1], SFM_READ, &infile_info);
 122         if(!infile)
 123                 err(1, "open");
 124         fprintf(stderr, "Input file has %ld frames, %d Hz, %d channels, format %08x, %d sections and is %s\n",
 125                 (long) infile_info.frames,
 126                 infile_info.samplerate,
 127                 infile_info.channels,
 128                 infile_info.format,
 129                 infile_info.sections,
 130                 infile_info.seekable ? "seekable" : "not seekable");
 131         chans = infile_info.channels;
 132
 133         sf_count_t size = MIN(infile_info.frames, strtod(argv[3], NULL) * infile_info.samplerate);
 134         sf_count_t guess = strtod(argv[4], NULL) * infile_info.samplerate;
 135         sf_count_t guess2 = strtod(argv[5], NULL) * infile_info.samplerate;
 136         int channels = infile_info.channels;
 137         size_t fftsize = atoi(argv[2]);
 138         size_t ndata = channels * (fftsize/2 + 1);
 139
 140         /*
 141         size_t ndata_lowpass = channels * (ndata / channels / 512);  // 43 Hz
 142         size_t ndata_highpass = channels * (ndata / channels / 4);   // 5512 Hz
 143         // simplifies the dot product and makes the target function more smooth
 144         */
 145         size_t ndata_lowpass = 0;
 146         //size_t ndata_highpass = channels * (ndata / channels / 4);   // 5512 Hz
 147         size_t ndata_highpass = ndata;
 148
 149         if(size < guess + 2 * (sf_count_t) fftsize)
 150                 err(1, "sound file too small (maybe reduce fftsize?)");
 151
 152         if(guess < fftsize)
 153                 err(1, "guess too close to file start (maybe reduce fftsize?)");
 154
 155         fprintf(stderr, "Using %ld frames of %d channels, guess at %ld, %d FFT size -> %d FFT result size\n", (long) size, channels, (long) guess, (int) fftsize, (int) ndata);
 156
 157         fftw_complex *data_end = fftw_malloc(sizeof(fftw_complex) * ndata);
 158         fftw_complex *data_cur = fftw_malloc(sizeof(fftw_complex) * ndata);
 159
 160         double *sbuf = malloc(sizeof(double) * (size * channels));
 161         if(size != sf_readf_double(infile, sbuf, size))
 162                 errx(1, "sf_read_double");
 163         sf_close(infile);
 164
 165         doFourier(sbuf + (size - fftsize) * chans, chans, fftsize, data_end);
 166         fprintf(stderr, "end transformed.\n");
 167
 168         double sxx = vectorDot(data_end + ndata_lowpass, data_end + ndata_lowpass, ndata_highpass - ndata_lowpass);
 169         if(sxx == 0)
 170                 errx(1, "ends with silence... use another end point");
 171
 172 #if 0
 173         {
 174                 sf_count_t i, j;
 175                 double sum[ndata];
 176                 double diffsum[ndata];
 177                 fftw_complex save[ndata];
 178                 for(i = guess; i < size - fftsize; ++i)
 179                 {
 180                         doFourier(sbuf + i * channels, channels, fftsize, data_cur);
 181                         for(j = 0; j < ndata; ++j)
 182                         {
 183                                 fftw_complex x = data_cur[j];
 184                                 if(i != 0)
 185                                 {
 186                                         fftw_complex y = save[j];
 187                                         sum[j] += cabs(x);
 188                                         diffsum[j] += cabs(y - x);
 189                                 }
 190                                 save[j] = x;
 191                         }
 192                         fprintf(stderr, "at position %d\n", (int) i);
 193                 }
 194                 for(j = 0; j < ndata; ++j)
 195                         printf("%d %.9f %.9f\n", j, sum[j], diffsum[j]);
 196                 return 0;
 197         }
 198 #endif
 199
 200         double similarityAt(sf_count_t i)
 201         {
 202                 // A trampoline! Wheeeeeeeeeew!
 203                 doFourier(sbuf + i * channels, channels, fftsize, data_cur);
 204                 double sxy = vectorDot(data_end + ndata_lowpass, data_cur + ndata_lowpass, ndata_highpass - ndata_lowpass);
 205                 double syy = vectorDot(data_cur + ndata_lowpass, data_cur + ndata_lowpass, ndata_highpass - ndata_lowpass);
 206                 double v = syy ? ((sxy*sxy) / (sxx*syy)) : -1;
 207                 //fprintf(stderr, "Evaluated at %.9f: %f\n", i / (double) infile_info.samplerate, v);
 208                 return v;
 209         }
 210
 211 #if 0
 212         {
 213                 sf_count_t i;
 214                 for(i = guess; i < size - 2 * fftsize; ++i)
 215                 {
 216                         printf("%.9f %f\n", i / (double) infile_info.samplerate, similarityAt(i));
 217                 }
 218                 return 0;
 219         }
 220 #endif
 221
 222         sf_count_t best = findMaximum(similarityAt, 0, guess - fftsize, guess2 - fftsize, size - 2 * fftsize);
 223         fprintf(stderr, "Result: %.9f (sample %ld)\n", (best + fftsize) / (double) infile_info.samplerate, (long) (best + fftsize));
 224
 225         // Now write it!
 226
 227         // 1. Crossfading end to our start sample
 228         fprintf(stderr, "Crossfading...\n");
 229         sf_count_t i;
 230         int j;
 231         double p = 2;
 232         for(j = 0; j < channels; ++j)
 233                 for(i = 0; i < fftsize; ++i)
 234                 {
 235                         double f1 = pow(fftsize - i, p);
 236                         double f2 = pow(i, p);
 237                         sbuf[(size - fftsize + i) * channels + j] =
 238                                 (
 239                                         sbuf[(size - fftsize + i) * channels + j] * f1
 240                                         +
 241                                         sbuf[(best + i) * channels + j] * f2
 242                                 ) / (f1 + f2);
 243                 }
 244
 245         // 2. Open sound file
 246         fprintf(stderr, "Opening...\n");
 247         SNDFILE *outfile = sf_open(argv[6], SFM_WRITE, &infile_info);
 248         if(!outfile)
 249                 err(1, "open");
 250
 251         fprintf(stderr, "Writing...\n");
 252         // 1. Write samples from start to just before start of our end FFT
 253         sf_writef_double(outfile, sbuf, size);
 254
 255         fprintf(stderr, "Closing...\n");
 256         sf_close(outfile);
 257
 258         printf("%ld %.9f\n", (long) (best + fftsize), (best + fftsize) / (double) infile_info.samplerate);
 259
 260         return 0;
 261 }