gst/audioresample/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 #include <string.h>
  25 #include <math.h>
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28
  29 #include <audioresample/functable.h>
  30 #include <audioresample/debug.h>
  31
  32
  33
  34 void
  35 functable_func_sinc (double *fx, double *dfx, double x, void *closure)
  36 {
  37   if (x == 0) {
  38     *fx = 1;
  39     *dfx = 0;
  40     return;
  41   }
  42
  43   *fx = sin (x) / x;
  44   *dfx = (cos (x) - sin (x) / x) / x;
  45 }
  46
  47 void
  48 functable_func_boxcar (double *fx, double *dfx, double x, void *closure)
  49 {
  50   double width = *(double *) closure;
  51
  52   if (x < width && x > -width) {
  53     *fx = 1;
  54   } else {
  55     *fx = 0;
  56   }
  57   *dfx = 0;
  58 }
  59
  60 void
  61 functable_func_hanning (double *fx, double *dfx, double x, void *closure)
  62 {
  63   double width = *(double *) closure;
  64
  65   if (x < width && x > -width) {
  66     x /= width;
  67     *fx = (1 - x * x) * (1 - x * x);
  68     *dfx = -2 * 2 * x / width * (1 - x * x);
  69   } else {
  70     *fx = 0;
  71     *dfx = 0;
  72   }
  73 }
  74
  75
  76 Functable *
  77 functable_new (void)
  78 {
  79   Functable *ft;
  80
  81   ft = malloc (sizeof (Functable));
  82   memset (ft, 0, sizeof (Functable));
  83
  84   return ft;
  85 }
  86
  87 void
  88 functable_free (Functable * ft)
  89 {
  90   free (ft);
  91 }
  92
  93 void
  94 functable_set_length (Functable * t, int length)
  95 {
  96   t->length = length;
  97 }
  98
  99 void
 100 functable_set_offset (Functable * t, double offset)
 101 {
 102   t->offset = offset;
 103 }
 104
 105 void
 106 functable_set_multiplier (Functable * t, double multiplier)
 107 {
 108   t->multiplier = multiplier;
 109 }
 110
 111 void
 112 functable_calculate (Functable * t, FunctableFunc func, void *closure)
 113 {
 114   int i;
 115   double x;
 116
 117   if (t->fx)
 118     free (t->fx);
 119   if (t->dfx)
 120     free (t->dfx);
 121
 122   t->fx = malloc (sizeof (double) * (t->length + 1));
 123   t->dfx = malloc (sizeof (double) * (t->length + 1));
 124
 125   t->inv_multiplier = 1.0 / t->multiplier;
 126
 127   for (i = 0; i < t->length + 1; i++) {
 128     x = t->offset + t->multiplier * i;
 129
 130     func (&t->fx[i], &t->dfx[i], x, closure);
 131   }
 132 }
 133
 134 void
 135 functable_calculate_multiply (Functable * t, FunctableFunc func, void *closure)
 136 {
 137   int i;
 138   double x;
 139
 140   for (i = 0; i < t->length + 1; i++) {
 141     double afx, adfx, bfx, bdfx;
 142
 143     afx = t->fx[i];
 144     adfx = t->dfx[i];
 145     x = t->offset + t->multiplier * i;
 146     func (&bfx, &bdfx, x, closure);
 147     t->fx[i] = afx * bfx;
 148     t->dfx[i] = afx * bdfx + adfx * bfx;
 149   }
 150
 151 }
 152
 153 double
 154 functable_evaluate (Functable * t, double x)
 155 {
 156   int i;
 157   double f0, f1, w0, w1;
 158   double x2, x3;
 159   double w;
 160
 161   if (x < t->offset || x > (t->offset + t->length * t->multiplier)) {
 162     RESAMPLE_DEBUG ("x out of range %g", x);
 163   }
 164
 165   x -= t->offset;
 166   x *= t->inv_multiplier;
 167   i = floor (x);
 168   x -= i;
 169
 170   x2 = x * x;
 171   x3 = x2 * x;
 172
 173   f1 = 3 * x2 - 2 * x3;
 174   f0 = 1 - f1;
 175   w0 = (x - 2 * x2 + x3) * t->multiplier;
 176   w1 = (-x2 + x3) * t->multiplier;
 177
 178   w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->dfx[i] * w0 + t->dfx[i + 1] * w1;
 179
 180   /*w = t->fx[i] * (1-x) + t->fx[i+1] * x; */
 181
 182   return w;
 183 }
 184
 185
 186 double
 187 functable_fir (Functable * t, double x, int n, double *data, int len)
 188 {
 189   int i, j;
 190   double f0, f1, w0, w1;
 191   double x2, x3;
 192   double w;
 193   double sum;
 194
 195   x -= t->offset;
 196   x /= t->multiplier;
 197   i = floor (x);
 198   x -= i;
 199
 200   x2 = x * x;
 201   x3 = x2 * x;
 202
 203   f1 = 3 * x2 - 2 * x3;
 204   f0 = 1 - f1;
 205   w0 = (x - 2 * x2 + x3) * t->multiplier;
 206   w1 = (-x2 + x3) * t->multiplier;
 207
 208   sum = 0;
 209   for (j = 0; j < len; j++) {
 210     w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->dfx[i] * w0 + t->dfx[i + 1] * w1;
 211     sum += data[j * 2] * w;
 212     i += n;
 213   }
 214
 215   return sum;
 216 }
 217
 218 void
 219 functable_fir2 (Functable * t, double *r0, double *r1, double x,
 220     int n, double *data, int len)
 221 {
 222   int i, j;
 223   double f0, f1, w0, w1;
 224   double x2, x3;
 225   double w;
 226   double sum0, sum1;
 227   double floor_x;
 228
 229   x -= t->offset;
 230   x *= t->inv_multiplier;
 231   floor_x = floor (x);
 232   i = floor_x;
 233   x -= floor_x;
 234
 235   x2 = x * x;
 236   x3 = x2 * x;
 237
 238   f1 = 3 * x2 - 2 * x3;
 239   f0 = 1 - f1;
 240   w0 = (x - 2 * x2 + x3) * t->multiplier;
 241   w1 = (-x2 + x3) * t->multiplier;
 242
 243   sum0 = 0;
 244   sum1 = 0;
 245   for (j = 0; j < len; j++) {
 246     w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->dfx[i] * w0 + t->dfx[i + 1] * w1;
 247     sum0 += data[j * 2] * w;
 248     sum1 += data[j * 2 + 1] * w;
 249     i += n;
 250   }
 251
 252   *r0 = sum0;
 253   *r1 = sum1;
 254 }