gst-libs/gst/resample/resample.c

   1 /* Resampling library
   2  * Copyright (C) <2001> David A. Schleef <ds@schleef.org>
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Library General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2 of the License, or any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  * License along with this library; if not, write to the
  16  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  17  * Boston, MA 02111-1307, USA.
  18  */
  19
  20
  21 #include <string.h>
  22 #include <math.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25
  26 #include "private.h"
  27 #include <gst/gstplugin.h>
  28 #include <gst/gstversion.h>
  29
  30 inline double sinc(double x)
  31 {
  32         if(x==0)return 1;
  33         return sin(x) / x;
  34 }
  35
  36 inline double window_func(double x)
  37 {
  38         x = 1 - x*x;
  39         return x*x;
  40 }
  41
  42 signed short double_to_s16(double x)
  43 {
  44         if(x<-32768){
  45                 printf("clipped\n");
  46                 return -32768;
  47         }
  48         if(x>32767){
  49                 printf("clipped\n");
  50                 return -32767;
  51         }
  52         return rint(x);
  53 }
  54
  55 signed short double_to_s16_ppcasm(double x)
  56 {
  57         if(x<-32768){
  58                 return -32768;
  59         }
  60         if(x>32767){
  61                 return -32767;
  62         }
  63         return rint(x);
  64 }
  65
  66 void resample_init(resample_t * r)
  67 {
  68         r->i_start = 0;
  69         if(r->filter_length&1){
  70                 r->o_start = 0;
  71         }else{
  72                 r->o_start = r->o_inc * 0.5;
  73         }
  74
  75         memset(r->acc, 0, sizeof(r->acc));
  76
  77         resample_reinit(r);
  78 }
  79
  80 void resample_reinit(resample_t * r)
  81 {
  82         /* i_inc is the number of samples that the output increments for
  83          * each input sample.  o_inc is the opposite. */
  84         r->i_inc = (double) r->o_rate / r->i_rate;
  85         r->o_inc = (double) r->i_rate / r->o_rate;
  86
  87         r->halftaps = (r->filter_length - 1.0) * 0.5;
  88
  89         if (r->format == RESAMPLE_S16) {
  90                 switch (r->method) {
  91                 default:
  92                 case RESAMPLE_NEAREST:
  93                         r->scale = resample_nearest_s16;
  94                         break;
  95                 case RESAMPLE_BILINEAR:
  96                         r->scale = resample_bilinear_s16;
  97                         break;
  98                 case RESAMPLE_SINC_SLOW:
  99                         r->scale = resample_sinc_s16;
 100                         break;
 101                 case RESAMPLE_SINC:
 102                         r->scale = resample_sinc_ft_s16;
 103                         break;
 104                 }
 105         } else if (r->format == RESAMPLE_FLOAT) {
 106                 switch (r->method) {
 107                 default:
 108                 case RESAMPLE_NEAREST:
 109                         r->scale = resample_nearest_float;
 110                         break;
 111                 case RESAMPLE_BILINEAR:
 112                         r->scale = resample_bilinear_float;
 113                         break;
 114                 case RESAMPLE_SINC_SLOW:
 115                         r->scale = resample_sinc_float;
 116                         break;
 117                 case RESAMPLE_SINC:
 118                         r->scale = resample_sinc_ft_float;
 119                         break;
 120                 }
 121         } else {
 122                 fprintf (stderr, "resample: Unexpected format \"%d\"\n", r->format);
 123         }
 124 }
 125
 126 /*
 127  * Prepare to be confused.
 128  *
 129  * We keep a "timebase" that is based on output samples.  The zero
 130  * of the timebase cooresponds to the next output sample that will
 131  * be written.
 132  *
 133  * i_start is the "time" that corresponds to the first input sample
 134  * in an incoming buffer.  Since the output depends on input samples
 135  * ahead in time, i_start will tend to be around halftaps.
 136  *
 137  * i_start_buf is the time of the first sample in the temporary
 138  * buffer.
 139  */
 140 void resample_scale(resample_t * r, void *i_buf, unsigned int i_size)
 141 {
 142         int o_size;
 143
 144         r->i_buf = i_buf;
 145
 146         r->i_samples = i_size / 2 / r->channels;
 147
 148         r->i_start_buf = r->i_start - r->filter_length * r->i_inc;
 149
 150         /* i_start is the offset (in a given output sample) that is the
 151          * beginning of the current input buffer */
 152         r->i_end = r->i_start + r->i_inc * r->i_samples;
 153
 154         r->o_samples = floor(r->i_end - r->halftaps * r->i_inc);
 155
 156         o_size = r->o_samples * r->channels * 2;
 157         r->o_buf = r->get_buffer(r->priv, o_size);
 158
 159         if(r->verbose){
 160                 printf("resample_scale: i_buf=%p i_size=%d\n",
 161                         i_buf,i_size);
 162                 printf("resample_scale: i_samples=%d o_samples=%d i_inc=%g o_buf=%p\n",
 163                         r->i_samples, r->o_samples, r->i_inc, r->o_buf);
 164                 printf("resample_scale: i_start=%g i_end=%g o_start=%g\n",
 165                         r->i_start, r->i_end, r->o_start);
 166         }
 167
 168         if ((r->filter_length + r->i_samples)*sizeof(double)*2 > r->buffer_len) {
 169                 int size = (r->filter_length + r->i_samples) * sizeof(double) * 2;
 170
 171                 if(r->verbose){
 172                         printf("resample temp buffer size=%d\n",size);
 173                 }
 174                 if(r->buffer)free(r->buffer);
 175                 r->buffer_len = size;
 176                 r->buffer = malloc(size);
 177                 memset(r->buffer, 0, size);
 178         }
 179
 180         if (r->format==RESAMPLE_S16) {
 181                 if(r->channels==2){
 182                         conv_double_short(
 183                                         r->buffer + r->filter_length * sizeof(double) * 2,
 184                                         r->i_buf, r->i_samples * 2);
 185                 } else {
 186                         conv_double_short_dstr(
 187                                         r->buffer + r->filter_length * sizeof(double) * 2,
 188                                         r->i_buf, r->i_samples, sizeof(double) * 2);
 189                 }
 190         } else if (r->format==RESAMPLE_FLOAT) {
 191                 if(r->channels==2){
 192                         conv_double_float(
 193                                         r->buffer + r->filter_length * sizeof(double) * 2,
 194                                         r->i_buf, r->i_samples * 2);
 195                 } else {
 196                         conv_double_float_dstr(
 197                                         r->buffer + r->filter_length * sizeof(double) * 2,
 198                                         r->i_buf, r->i_samples, sizeof(double) * 2);
 199                 }
 200         }
 201
 202         r->scale(r);
 203
 204         memcpy(r->buffer,
 205                 r->buffer + r->i_samples * sizeof(double) * 2,
 206                 r->filter_length * sizeof(double) * 2);
 207
 208         /* updating times */
 209         r->i_start += r->i_samples * r->i_inc;
 210         r->o_start += r->o_samples * r->o_inc - r->i_samples;
 211
 212         /* adjusting timebase zero */
 213         r->i_start -= r->o_samples;
 214 }
 215
 216 void resample_nearest_s16(resample_t * r)
 217 {
 218         signed short *i_ptr, *o_ptr;
 219         int i_count = 0;
 220         double a;
 221         int i;
 222
 223         i_ptr = (signed short *) r->i_buf;
 224         o_ptr = (signed short *) r->o_buf;
 225
 226         a = r->o_start;
 227         i_count = 0;
 228 #define SCALE_LOOP(COPY,INC) \
 229         for (i = 0; i < r->o_samples; i++) {    \
 230                 COPY;                                                   \
 231                 a += r->o_inc;                                          \
 232                 while (a >= 1) {                                \
 233                         a -= 1;                                         \
 234                         i_ptr+=INC;                                     \
 235                         i_count++;                                      \
 236                 }                                                               \
 237                 o_ptr+=INC;                                             \
 238         }
 239
 240         switch (r->channels) {
 241         case 1:
 242                 SCALE_LOOP(o_ptr[0] = i_ptr[0], 1);
 243                 break;
 244         case 2:
 245                 SCALE_LOOP(o_ptr[0] = i_ptr[0];
 246                            o_ptr[1] = i_ptr[1], 2);
 247                 break;
 248         default:
 249         {
 250                 int n, n_chan = r->channels;
 251
 252                 SCALE_LOOP(for (n = 0; n < n_chan; n++) o_ptr[n] =
 253                            i_ptr[n], n_chan);
 254         }
 255         }
 256         if (i_count != r->i_samples) {
 257                 printf("handled %d in samples (expected %d)\n", i_count,
 258                        r->i_samples);
 259         }
 260 }
 261
 262 void resample_bilinear_s16(resample_t * r)
 263 {
 264         signed short *i_ptr, *o_ptr;
 265         int o_count = 0;
 266         double b;
 267         int i;
 268         double acc0, acc1;
 269
 270         i_ptr = (signed short *) r->i_buf;
 271         o_ptr = (signed short *) r->o_buf;
 272
 273         acc0 = r->acc[0];
 274         acc1 = r->acc[1];
 275         b = r->i_start;
 276         for (i = 0; i < r->i_samples; i++) {
 277                 b += r->i_inc;
 278                 /*printf("in %d\n",i_ptr[0]); */
 279                 if(b>=2){
 280                         printf("not expecting b>=2\n");
 281                 }
 282                 if (b >= 1) {
 283                         acc0 += (1.0 - (b-r->i_inc)) * i_ptr[0];
 284                         acc1 += (1.0 - (b-r->i_inc)) * i_ptr[1];
 285
 286                         o_ptr[0] = rint(acc0);
 287                         /*printf("out %d\n",o_ptr[0]); */
 288                         o_ptr[1] = rint(acc1);
 289                         o_ptr += 2;
 290                         o_count++;
 291
 292                         b -= 1.0;
 293
 294                         acc0 = b * i_ptr[0];
 295                         acc1 = b * i_ptr[1];
 296                 } else {
 297                         acc0 += i_ptr[0] * r->i_inc;
 298                         acc1 += i_ptr[1] * r->i_inc;
 299                 }
 300                 i_ptr += 2;
 301         }
 302         r->acc[0] = acc0;
 303         r->acc[1] = acc1;
 304
 305         if (o_count != r->o_samples) {
 306                 printf("handled %d out samples (expected %d)\n", o_count,
 307                        r->o_samples);
 308         }
 309 }
 310
 311 void resample_sinc_slow_s16(resample_t * r)
 312 {
 313         signed short *i_ptr, *o_ptr;
 314         int i, j;
 315         double c0, c1;
 316         double a;
 317         int start;
 318         double center;
 319         double weight;
 320
 321         if (!r->buffer) {
 322                 int size = r->filter_length * 2 * r->channels;
 323
 324                 printf("resample temp buffer\n");
 325                 r->buffer = malloc(size);
 326                 memset(r->buffer, 0, size);
 327         }
 328
 329         i_ptr = (signed short *) r->i_buf;
 330         o_ptr = (signed short *) r->o_buf;
 331
 332         a = r->i_start;
 333 #define GETBUF(index,chan) (((index)<0) \
 334                         ? ((short *)(r->buffer))[((index)+r->filter_length)*2+(chan)] \
 335                         : i_ptr[(index)*2+(chan)])
 336         {
 337                 double sinx, cosx, sind, cosd;
 338                 double x, d;
 339                 double t;
 340
 341                 for (i = 0; i < r->o_samples; i++) {
 342                         start = floor(a) - r->filter_length;
 343                         center = a - r->halftaps;
 344                         x = M_PI * (start - center) * r->o_inc;
 345                         sinx = sin(M_PI * (start - center) * r->o_inc);
 346                         cosx = cos(M_PI * (start - center) * r->o_inc);
 347                         d = M_PI * r->o_inc;
 348                         sind = sin(M_PI * r->o_inc);
 349                         cosd = cos(M_PI * r->o_inc);
 350                         c0 = 0;
 351                         c1 = 0;
 352                         for (j = 0; j < r->filter_length; j++) {
 353                                 weight = (x==0)?1:(sinx/x);
 354 /*printf("j %d sin %g cos %g\n",j,sinx,cosx); */
 355 /*printf("j %d sin %g x %g sinc %g\n",j,sinx,x,weight); */
 356                                 c0 += weight * GETBUF((start + j), 0);
 357                                 c1 += weight * GETBUF((start + j), 1);
 358                                 t = cosx * cosd - sinx * sind;
 359                                 sinx = cosx * sind + sinx * cosd;
 360                                 cosx = t;
 361                                 x += d;
 362                         }
 363                         o_ptr[0] = rint(c0);
 364                         o_ptr[1] = rint(c1);
 365                         o_ptr += 2;
 366                         a += r->o_inc;
 367                 }
 368         }
 369 #undef GETBUF
 370
 371         memcpy(r->buffer,
 372                i_ptr + (r->i_samples - r->filter_length) * r->channels,
 373                r->filter_length * 2 * r->channels);
 374 }
 375
 376 /* only works for channels == 2 ???? */
 377 void resample_sinc_s16(resample_t * r)
 378 {
 379         double *ptr;
 380         signed short *o_ptr;
 381         int i, j;
 382         double c0, c1;
 383         double a;
 384         int start;
 385         double center;
 386         double weight;
 387         double x0, x, d;
 388         double scale;
 389
 390         ptr = (double *) r->buffer;
 391         o_ptr = (signed short *) r->o_buf;
 392
 393         /* scale provides a cutoff frequency for the low
 394          * pass filter aspects of sinc().  scale=M_PI
 395          * will cut off at the input frequency, which is
 396          * good for up-sampling, but will cause aliasing
 397          * for downsampling.  Downsampling needs to be
 398          * cut off at o_rate, thus scale=M_PI*r->i_inc. */
 399         /* actually, it needs to be M_PI*r->i_inc*r->i_inc.
 400          * Need to research why. */
 401         scale = M_PI*r->i_inc;
 402         for (i = 0; i < r->o_samples; i++) {
 403                 a = r->o_start + i * r->o_inc;
 404                 start = floor(a - r->halftaps);
 405 /*printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); */
 406                 center = a;
 407                 /*x = M_PI * (start - center) * r->o_inc; */
 408                 /*d = M_PI * r->o_inc; */
 409                 /*x = (start - center) * r->o_inc; */
 410                 x0 = (start - center) * r->o_inc;
 411                 d = r->o_inc;
 412                 c0 = 0;
 413                 c1 = 0;
 414                 for (j = 0; j < r->filter_length; j++) {
 415                         x = x0 + d * j;
 416                         weight = sinc(x*scale*r->i_inc)*scale/M_PI;
 417                         weight *= window_func(x/r->halftaps*r->i_inc);
 418                         c0 += weight * ptr[(start + j + r->filter_length)*2 + 0];
 419                         c1 += weight * ptr[(start + j + r->filter_length)*2 + 1];
 420                 }
 421                 o_ptr[0] = double_to_s16(c0);
 422                 o_ptr[1] = double_to_s16(c1);
 423                 o_ptr += 2;
 424         }
 425 }
 426
 427 /*
 428  * Resampling audio is best done using a sinc() filter.
 429  *
 430  *
 431  *  out[t] = Sum( in[t'] * sinc((t-t')/delta_t), all t')
 432  *
 433  * The immediate problem with this algorithm is that it involves a
 434  * sum over an infinite number of input samples, both in the past
 435  * and future.  Note that even though sinc(x) is bounded by 1/x,
 436  * and thus decays to 0 for large x, since sum(x,{x=0,1..,n}) diverges
 437  * as log(n), we need to be careful about convergence.  This is
 438  * typically done by using a windowing function, which also makes
 439  * the sum over a finite number of input samples.
 440  *
 441  * The next problem is computational:  sinc(), and especially
 442  * sinc() multiplied by a non-trivial windowing function is expensive
 443  * to calculate, and also difficult to find SIMD optimizations.  Since
 444  * the time increment on input and output is different, it is not
 445  * possible to use a FIR filter, because the taps would have to be
 446  * recalculated for every t.
 447  *
 448  * To get around the expense of calculating sinc() for every point,
 449  * we pre-calculate sinc() at a number of points, and then interpolate
 450  * for the values we want in calculations.  The interpolation method
 451  * chosen is bi-cubic, which requires both the evalated function and
 452  * its derivative at every pre-sampled point.  Also, if the sampled
 453  * points are spaced commensurate with the input delta_t, we notice
 454  * that the interpolating weights are the same for every input point.
 455  * This decreases the number of operations to 4 multiplies and 4 adds
 456  * for each tap, regardless of the complexity of the filtering function.
 457  *
 458  * At this point, it is possible to rearrange the problem as the sum
 459  * of 4 properly weghted FIR filters.  Typical SIMD computation units
 460  * are highly optimized for FIR filters, making long filter lengths
 461  * reasonable.
 462  */
 463
 464 static functable_t *ft;
 465
 466 double out_tmp[10000];
 467
 468 void resample_sinc_ft_s16(resample_t * r)
 469 {
 470         double *ptr;
 471         signed short *o_ptr;
 472         int i;
 473         /*int j; */
 474         double c0, c1;
 475         /*double a; */
 476         double start_f, start_x;
 477         int start;
 478         double center;
 479         /*double weight; */
 480         double x, d;
 481         double scale;
 482         int n = 4;
 483
 484         scale = r->i_inc;       /* cutoff at 22050 */
 485         /*scale = 1.0;          // cutoff at 24000 */
 486         /*scale = r->i_inc * 0.5;       // cutoff at 11025 */
 487
 488         if(!ft){
 489                 ft = malloc(sizeof(*ft));
 490                 memset(ft,0,sizeof(*ft));
 491
 492                 ft->len = (r->filter_length + 2) * n;
 493                 ft->offset = 1.0 / n;
 494                 ft->start = - ft->len * 0.5 * ft->offset;
 495
 496                 ft->func_x = functable_sinc;
 497                 ft->func_dx = functable_dsinc;
 498                 ft->scale = M_PI * scale;
 499
 500                 ft->func2_x = functable_window_std;
 501                 ft->func2_dx = functable_window_dstd;
 502                 ft->scale2 = 1.0 / r->halftaps;
 503
 504                 functable_init(ft);
 505
 506                 /*printf("len=%d offset=%g start=%g\n",ft->len,ft->offset,ft->start); */
 507         }
 508
 509         ptr = r->buffer;
 510         o_ptr = (signed short *) r->o_buf;
 511
 512         center = r->o_start;
 513         start_x = center - r->halftaps;
 514         start_f = floor(start_x);
 515         start_x -= start_f;
 516         start = start_f;
 517         for (i = 0; i < r->o_samples; i++) {
 518                 /*start_f = floor(center - r->halftaps); */
 519 /*printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); */
 520                 x = start_f - center;
 521                 d = 1;
 522                 c0 = 0;
 523                 c1 = 0;
 524 /*#define slow */
 525 #ifdef slow
 526                 for (j = 0; j < r->filter_length; j++) {
 527                         weight = functable_eval(ft,x)*scale;
 528                         /*weight = sinc(M_PI * scale * x)*scale*r->i_inc; */
 529                         /*weight *= window_func(x / r->halftaps); */
 530                         c0 += weight * ptr[(start + j + r->filter_length)*2 + 0];
 531                         c1 += weight * ptr[(start + j + r->filter_length)*2 + 1];
 532                         x += d;
 533                 }
 534 #else
 535                 functable_fir2(ft,
 536                         &c0,&c1,
 537                         x, n,
 538                         ptr+(start + r->filter_length)*2,
 539                         r->filter_length);
 540                 c0 *= scale;
 541                 c1 *= scale;
 542 #endif
 543
 544                 out_tmp[2 * i + 0] = c0;
 545                 out_tmp[2 * i + 1] = c1;
 546                 center += r->o_inc;
 547                 start_x += r->o_inc;
 548                 while(start_x>=1.0){
 549                         start_f++;
 550                         start_x -= 1.0;
 551                         start++;
 552                 }
 553         }
 554
 555         if(r->channels==2){
 556                 conv_short_double(r->o_buf,out_tmp,2 * r->o_samples);
 557         }else{
 558                 conv_short_double_sstr(r->o_buf,out_tmp,r->o_samples,2 * sizeof(double));
 559         }
 560 }
 561
 562 /********
 563  ** float code below
 564  ********/
 565
 566
 567 void resample_nearest_float(resample_t * r)
 568 {
 569         float *i_ptr, *o_ptr;
 570         int i_count = 0;
 571         double a;
 572         int i;
 573
 574         i_ptr = (float *) r->i_buf;
 575         o_ptr = (float *) r->o_buf;
 576
 577         a = r->o_start;
 578         i_count = 0;
 579 #define SCALE_LOOP(COPY,INC) \
 580         for (i = 0; i < r->o_samples; i++) {    \
 581                 COPY;                                                   \
 582                 a += r->o_inc;                                          \
 583                 while (a >= 1) {                                \
 584                         a -= 1;                                         \
 585                         i_ptr+=INC;                                     \
 586                         i_count++;                                      \
 587                 }                                                               \
 588                 o_ptr+=INC;                                             \
 589         }
 590
 591         switch (r->channels) {
 592         case 1:
 593                 SCALE_LOOP(o_ptr[0] = i_ptr[0], 1);
 594                 break;
 595         case 2:
 596                 SCALE_LOOP(o_ptr[0] = i_ptr[0];
 597                            o_ptr[1] = i_ptr[1], 2);
 598                 break;
 599         default:
 600         {
 601                 int n, n_chan = r->channels;
 602
 603                 SCALE_LOOP(for (n = 0; n < n_chan; n++) o_ptr[n] =
 604                            i_ptr[n], n_chan);
 605         }
 606         }
 607         if (i_count != r->i_samples) {
 608                 printf("handled %d in samples (expected %d)\n", i_count,
 609                        r->i_samples);
 610         }
 611 }
 612
 613 void resample_bilinear_float(resample_t * r)
 614 {
 615         float *i_ptr, *o_ptr;
 616         int o_count = 0;
 617         double b;
 618         int i;
 619         double acc0, acc1;
 620
 621         i_ptr = (float *) r->i_buf;
 622         o_ptr = (float *) r->o_buf;
 623
 624         acc0 = r->acc[0];
 625         acc1 = r->acc[1];
 626         b = r->i_start;
 627         for (i = 0; i < r->i_samples; i++) {
 628                 b += r->i_inc;
 629                 /*printf("in %d\n",i_ptr[0]); */
 630                 if(b>=2){
 631                         printf("not expecting b>=2\n");
 632                 }
 633                 if (b >= 1) {
 634                         acc0 += (1.0 - (b-r->i_inc)) * i_ptr[0];
 635                         acc1 += (1.0 - (b-r->i_inc)) * i_ptr[1];
 636
 637                         o_ptr[0] = acc0;
 638                         /*printf("out %d\n",o_ptr[0]); */
 639                         o_ptr[1] = acc1;
 640                         o_ptr += 2;
 641                         o_count++;
 642
 643                         b -= 1.0;
 644
 645                         acc0 = b * i_ptr[0];
 646                         acc1 = b * i_ptr[1];
 647                 } else {
 648                         acc0 += i_ptr[0] * r->i_inc;
 649                         acc1 += i_ptr[1] * r->i_inc;
 650                 }
 651                 i_ptr += 2;
 652         }
 653         r->acc[0] = acc0;
 654         r->acc[1] = acc1;
 655
 656         if (o_count != r->o_samples) {
 657                 printf("handled %d out samples (expected %d)\n", o_count,
 658                        r->o_samples);
 659         }
 660 }
 661
 662 void resample_sinc_slow_float(resample_t * r)
 663 {
 664         float *i_ptr, *o_ptr;
 665         int i, j;
 666         double c0, c1;
 667         double a;
 668         int start;
 669         double center;
 670         double weight;
 671
 672         if (!r->buffer) {
 673                 int size = r->filter_length * sizeof(float) * r->channels;
 674
 675                 printf("resample temp buffer\n");
 676                 r->buffer = malloc(size);
 677                 memset(r->buffer, 0, size);
 678         }
 679
 680         i_ptr = (float *) r->i_buf;
 681         o_ptr = (float *) r->o_buf;
 682
 683         a = r->i_start;
 684 #define GETBUF(index,chan) (((index)<0) \
 685                         ? ((float *)(r->buffer))[((index)+r->filter_length)*2+(chan)] \
 686                         : i_ptr[(index)*2+(chan)])
 687         {
 688                 double sinx, cosx, sind, cosd;
 689                 double x, d;
 690                 double t;
 691
 692                 for (i = 0; i < r->o_samples; i++) {
 693                         start = floor(a) - r->filter_length;
 694                         center = a - r->halftaps;
 695                         x = M_PI * (start - center) * r->o_inc;
 696                         sinx = sin(M_PI * (start - center) * r->o_inc);
 697                         cosx = cos(M_PI * (start - center) * r->o_inc);
 698                         d = M_PI * r->o_inc;
 699                         sind = sin(M_PI * r->o_inc);
 700                         cosd = cos(M_PI * r->o_inc);
 701                         c0 = 0;
 702                         c1 = 0;
 703                         for (j = 0; j < r->filter_length; j++) {
 704                                 weight = (x==0)?1:(sinx/x);
 705 /*printf("j %d sin %g cos %g\n",j,sinx,cosx); */
 706 /*printf("j %d sin %g x %g sinc %g\n",j,sinx,x,weight); */
 707                                 c0 += weight * GETBUF((start + j), 0);
 708                                 c1 += weight * GETBUF((start + j), 1);
 709                                 t = cosx * cosd - sinx * sind;
 710                                 sinx = cosx * sind + sinx * cosd;
 711                                 cosx = t;
 712                                 x += d;
 713                         }
 714                         o_ptr[0] = c0;
 715                         o_ptr[1] = c1;
 716                         o_ptr += 2;
 717                         a += r->o_inc;
 718                 }
 719         }
 720 #undef GETBUF
 721
 722         memcpy(r->buffer,
 723                i_ptr + (r->i_samples - r->filter_length) * r->channels,
 724                r->filter_length * sizeof(float) * r->channels);
 725 }
 726
 727 /* only works for channels == 2 ???? */
 728 void resample_sinc_float(resample_t * r)
 729 {
 730         double *ptr;
 731         float *o_ptr;
 732         int i, j;
 733         double c0, c1;
 734         double a;
 735         int start;
 736         double center;
 737         double weight;
 738         double x0, x, d;
 739         double scale;
 740
 741         ptr = (double *) r->buffer;
 742         o_ptr = (float *) r->o_buf;
 743
 744         /* scale provides a cutoff frequency for the low
 745          * pass filter aspects of sinc().  scale=M_PI
 746          * will cut off at the input frequency, which is
 747          * good for up-sampling, but will cause aliasing
 748          * for downsampling.  Downsampling needs to be
 749          * cut off at o_rate, thus scale=M_PI*r->i_inc. */
 750         /* actually, it needs to be M_PI*r->i_inc*r->i_inc.
 751          * Need to research why. */
 752         scale = M_PI*r->i_inc;
 753         for (i = 0; i < r->o_samples; i++) {
 754                 a = r->o_start + i * r->o_inc;
 755                 start = floor(a - r->halftaps);
 756 /*printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); */
 757                 center = a;
 758                 /*x = M_PI * (start - center) * r->o_inc; */
 759                 /*d = M_PI * r->o_inc; */
 760                 /*x = (start - center) * r->o_inc; */
 761                 x0 = (start - center) * r->o_inc;
 762                 d = r->o_inc;
 763                 c0 = 0;
 764                 c1 = 0;
 765                 for (j = 0; j < r->filter_length; j++) {
 766                         x = x0 + d * j;
 767                         weight = sinc(x*scale*r->i_inc)*scale/M_PI;
 768                         weight *= window_func(x/r->halftaps*r->i_inc);
 769                         c0 += weight * ptr[(start + j + r->filter_length)*2 + 0];
 770                         c1 += weight * ptr[(start + j + r->filter_length)*2 + 1];
 771                 }
 772                 o_ptr[0] = c0;
 773                 o_ptr[1] = c1;
 774                 o_ptr += 2;
 775         }
 776 }
 777
 778 void resample_sinc_ft_float(resample_t * r)
 779 {
 780         double *ptr;
 781         float *o_ptr;
 782         int i;
 783         /*int j; */
 784         double c0, c1;
 785         /*double a; */
 786         double start_f, start_x;
 787         int start;
 788         double center;
 789         /*double weight; */
 790         double x, d;
 791         double scale;
 792         int n = 4;
 793
 794         scale = r->i_inc;       /* cutoff at 22050 */
 795         /*scale = 1.0;          // cutoff at 24000 */
 796         /*scale = r->i_inc * 0.5;       // cutoff at 11025 */
 797
 798         if(!ft){
 799                 ft = malloc(sizeof(*ft));
 800                 memset(ft,0,sizeof(*ft));
 801
 802                 ft->len = (r->filter_length + 2) * n;
 803                 ft->offset = 1.0 / n;
 804                 ft->start = - ft->len * 0.5 * ft->offset;
 805
 806                 ft->func_x = functable_sinc;
 807                 ft->func_dx = functable_dsinc;
 808                 ft->scale = M_PI * scale;
 809
 810                 ft->func2_x = functable_window_std;
 811                 ft->func2_dx = functable_window_dstd;
 812                 ft->scale2 = 1.0 / r->halftaps;
 813
 814                 functable_init(ft);
 815
 816                 /*printf("len=%d offset=%g start=%g\n",ft->len,ft->offset,ft->start); */
 817         }
 818
 819         ptr = r->buffer;
 820         o_ptr = (float *) r->o_buf;
 821
 822         center = r->o_start;
 823         start_x = center - r->halftaps;
 824         start_f = floor(start_x);
 825         start_x -= start_f;
 826         start = start_f;
 827         for (i = 0; i < r->o_samples; i++) {
 828                 /*start_f = floor(center - r->halftaps); */
 829 /*printf("%d: a=%g start=%d end=%d\n",i,a,start,start+r->filter_length-1); */
 830                 x = start_f - center;
 831                 d = 1;
 832                 c0 = 0;
 833                 c1 = 0;
 834 /*#define slow */
 835 #ifdef slow
 836                 for (j = 0; j < r->filter_length; j++) {
 837                         weight = functable_eval(ft,x)*scale;
 838                         /*weight = sinc(M_PI * scale * x)*scale*r->i_inc; */
 839                         /*weight *= window_func(x / r->halftaps); */
 840                         c0 += weight * ptr[(start + j + r->filter_length)*2 + 0];
 841                         c1 += weight * ptr[(start + j + r->filter_length)*2 + 1];
 842                         x += d;
 843                 }
 844 #else
 845                 functable_fir2(ft,
 846                         &c0,&c1,
 847                         x, n,
 848                         ptr+(start + r->filter_length)*2,
 849                         r->filter_length);
 850                 c0 *= scale;
 851                 c1 *= scale;
 852 #endif
 853
 854                 out_tmp[2 * i + 0] = c0;
 855                 out_tmp[2 * i + 1] = c1;
 856                 center += r->o_inc;
 857                 start_x += r->o_inc;
 858                 while(start_x>=1.0){
 859                         start_f++;
 860                         start_x -= 1.0;
 861                         start++;
 862                 }
 863         }
 864
 865         if(r->channels==2){
 866                 conv_float_double(r->o_buf,out_tmp,2 * r->o_samples);
 867         }else{
 868                 conv_float_double_sstr(r->o_buf,out_tmp,r->o_samples,2 * sizeof(double));
 869         }
 870 }
 871
 872 static gboolean
 873 plugin_init (GstPlugin *plugin)
 874 {
 875   return TRUE;
 876 }
 877
 878 GST_PLUGIN_DEFINE (
 879   GST_VERSION_MAJOR,
 880   GST_VERSION_MINOR,
 881   "gstresample",
 882   "Resampling routines for use in audio plugins",
 883   plugin_init,
 884   VERSION,
 885   GST_LICENSE,
 886   GST_COPYRIGHT,
 887   GST_PACKAGE,
 888   GST_ORIGIN
 889 );
 890