gst/audioresample/resample_functable.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 #ifdef HAVE_CONFIG_H
  21 #include <config.h>
  22 #endif
  23
  24
  25 #include <string.h>
  26 #include <math.h>
  27 #include <stdio.h>
  28 #include <stdlib.h>
  29 #include <limits.h>
  30 #include <liboil/liboil.h>
  31
  32 #include <audioresample/resample.h>
  33 #include <audioresample/buffer.h>
  34 #include <audioresample/debug.h>
  35
  36 static void
  37 func_sinc (double *fx, double *dfx, double x, void *closure)
  38 {
  39   //double scale = *(double *)closure;
  40   double scale = M_PI;
  41
  42   if (x == 0) {
  43     *fx = 1;
  44     *dfx = 0;
  45     return;
  46   }
  47
  48   x *= scale;
  49   *fx = sin (x) / x;
  50   *dfx = scale * (cos (x) - sin (x) / x) / x;
  51 }
  52
  53 static void
  54 func_hanning (double *fx, double *dfx, double x, void *closure)
  55 {
  56   double width = *(double *) closure;
  57
  58   if (x < width && x > -width) {
  59     x /= width;
  60     *fx = (1 - x * x) * (1 - x * x);
  61     *dfx = -2 * 2 * x / width * (1 - x * x);
  62   } else {
  63     *fx = 0;
  64     *dfx = 0;
  65   }
  66 }
  67
  68 #if 0
  69 static double
  70 resample_sinc_window (double x, double halfwidth, double scale)
  71 {
  72   double y;
  73
  74   if (x == 0)
  75     return 1.0;
  76   if (x < -halfwidth || x > halfwidth)
  77     return 0.0;
  78
  79   y = sin (x * M_PI * scale) / (x * M_PI * scale) * scale;
  80
  81   x /= halfwidth;
  82   y *= (1 - x * x) * (1 - x * x);
  83
  84   return y;
  85 }
  86 #endif
  87
  88 #if 0
  89 static void
  90 functable_test (Functable * ft, double halfwidth)
  91 {
  92   int i;
  93   double x;
  94
  95   for (i = 0; i < 100; i++) {
  96     x = i * 0.1;
  97     printf ("%d %g %g\n", i, resample_sinc_window (x, halfwidth, 1.0),
  98         functable_evaluate (ft, x));
  99   }
 100   exit (0);
 101
 102 }
 103 #endif
 104
 105
 106 void
 107 resample_scale_functable (ResampleState * r)
 108 {
 109   if (r->need_reinit) {
 110     double hanning_width;
 111
 112     r->sample_size = r->n_channels * resample_format_size (r->format);
 113     RESAMPLE_DEBUG ("sample size %d", r->sample_size);
 114
 115     if (r->buffer)
 116       free (r->buffer);
 117     r->buffer_len = r->sample_size * r->filter_length;
 118     r->buffer = malloc (r->buffer_len);
 119     memset (r->buffer, 0, r->buffer_len);
 120
 121     r->i_inc = r->o_rate / r->i_rate;
 122     r->o_inc = r->i_rate / r->o_rate;
 123     RESAMPLE_DEBUG ("i_inc %g o_inc %g", r->i_inc, r->o_inc);
 124
 125     r->i_start = -r->i_inc * r->filter_length;
 126
 127     if (r->ft) {
 128       functable_free (r->ft);
 129     }
 130     r->ft = functable_new ();
 131     functable_set_length (r->ft, r->filter_length * 16);
 132     functable_set_offset (r->ft, -r->filter_length / 2);
 133     functable_set_multiplier (r->ft, 1 / 16.0);
 134
 135     hanning_width = r->filter_length / 2;
 136     functable_calculate (r->ft, func_sinc, NULL);
 137     functable_calculate_multiply (r->ft, func_hanning, &hanning_width);
 138
 139     //functable_test(r->ft, 0.5 * r->filter_length);
 140 #if 0
 141     if (r->i_inc < 1.0) {
 142       r->sinc_scale = r->i_inc;
 143       if (r->sinc_scale == 0.5) {
 144         /* strange things happen at integer multiples */
 145         r->sinc_scale = 1.0;
 146       }
 147     } else {
 148       r->sinc_scale = 1.0;
 149     }
 150 #else
 151     r->sinc_scale = 1.0;
 152 #endif
 153
 154     r->need_reinit = 0;
 155   }
 156
 157   while (r->o_size > 0) {
 158     double midpoint;
 159     int i;
 160     int j;
 161
 162     RESAMPLE_DEBUG ("i_start %g", r->i_start);
 163     midpoint = r->i_start + (r->filter_length - 1) * 0.5 * r->i_inc;
 164     if (midpoint > 0.5 * r->i_inc) {
 165       RESAMPLE_ERROR ("inconsistent state");
 166     }
 167     while (midpoint < -0.5 * r->i_inc) {
 168       AudioresampleBuffer *buffer;
 169
 170       buffer = audioresample_buffer_queue_pull (r->queue, r->sample_size);
 171       if (buffer == NULL) {
 172         RESAMPLE_ERROR ("buffer_queue_pull returned NULL");
 173         return;
 174       }
 175
 176       r->i_start += r->i_inc;
 177       RESAMPLE_DEBUG ("pulling (i_start = %g)", r->i_start);
 178
 179       midpoint += r->i_inc;
 180       memmove (r->buffer, r->buffer + r->sample_size,
 181           r->buffer_len - r->sample_size);
 182
 183       memcpy (r->buffer + r->buffer_len - r->sample_size, buffer->data,
 184           r->sample_size);
 185       audioresample_buffer_unref (buffer);
 186     }
 187
 188     switch (r->format) {
 189       case RESAMPLE_FORMAT_S16:
 190         for (i = 0; i < r->n_channels; i++) {
 191           double acc = 0;
 192           double offset;
 193           double x;
 194
 195           for (j = 0; j < r->filter_length; j++) {
 196             offset = (r->i_start + j * r->i_inc) * r->o_inc;
 197             x = *(int16_t *) (r->buffer + i * sizeof (int16_t) +
 198                 j * r->sample_size);
 199             acc += functable_evaluate (r->ft, offset) * x;
 200             //acc += resample_sinc_window (offset, r->filter_length * 0.5, r->sinc_scale) * x;
 201           }
 202           if (acc < -32768.0)
 203             acc = -32768.0;
 204           if (acc > 32767.0)
 205             acc = 32767.0;
 206
 207           *(int16_t *) (r->o_buf + i * sizeof (int16_t)) = rint (acc);
 208         }
 209         break;
 210       case RESAMPLE_FORMAT_S32:
 211         for (i = 0; i < r->n_channels; i++) {
 212           double acc = 0;
 213           double offset;
 214           double x;
 215
 216           for (j = 0; j < r->filter_length; j++) {
 217             offset = (r->i_start + j * r->i_inc) * r->o_inc;
 218             x = *(int32_t *) (r->buffer + i * sizeof (int32_t) +
 219                 j * r->sample_size);
 220             acc += functable_evaluate (r->ft, offset) * x;
 221             //acc += resample_sinc_window (offset, r->filter_length * 0.5, r->sinc_scale) * x;
 222           }
 223           if (acc < -2147483648.0)
 224             acc = -2147483648.0;
 225           if (acc > 2147483647.0)
 226             acc = 2147483647.0;
 227
 228           *(int32_t *) (r->o_buf + i * sizeof (int32_t)) = rint (acc);
 229         }
 230         break;
 231       case RESAMPLE_FORMAT_F32:
 232         for (i = 0; i < r->n_channels; i++) {
 233           double acc = 0;
 234           double offset;
 235           double x;
 236
 237           for (j = 0; j < r->filter_length; j++) {
 238             offset = (r->i_start + j * r->i_inc) * r->o_inc;
 239             x = *(float *) (r->buffer + i * sizeof (float) +
 240                 j * r->sample_size);
 241             acc += functable_evaluate (r->ft, offset) * x;
 242             //acc += resample_sinc_window (offset, r->filter_length * 0.5, r->sinc_scale) * x;
 243           }
 244
 245           *(float *) (r->o_buf + i * sizeof (float)) = acc;
 246         }
 247         break;
 248       case RESAMPLE_FORMAT_F64:
 249         for (i = 0; i < r->n_channels; i++) {
 250           double acc = 0;
 251           double offset;
 252           double x;
 253
 254           for (j = 0; j < r->filter_length; j++) {
 255             offset = (r->i_start + j * r->i_inc) * r->o_inc;
 256             x = *(double *) (r->buffer + i * sizeof (double) +
 257                 j * r->sample_size);
 258             acc += functable_evaluate (r->ft, offset) * x;
 259             //acc += resample_sinc_window (offset, r->filter_length * 0.5, r->sinc_scale) * x;
 260           }
 261
 262           *(double *) (r->o_buf + i * sizeof (double)) = acc;
 263         }
 264         break;
 265     }
 266
 267     r->i_start -= 1.0;
 268     r->o_buf += r->sample_size;
 269     r->o_size -= r->sample_size;
 270   }
 271
 272 }