src/mcompand_xover.h

   1 /* libSoX Compander Crossover Filter  (c) 2008 robs@users.sourceforge.net
   2  *
   3  * This library is free software; you can redistribute it and/or modify it
   4  * under the terms of the GNU Lesser General Public License as published by
   5  * the Free Software Foundation; either version 2.1 of the License, or (at
   6  * your option) any later version.
   7  *
   8  * This library is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU Lesser General Public License
  14  * along with this library; if not, write to the Free Software Foundation,
  15  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  16  */
  17
  18 #define N 4          /* 4th order Linkwitz-Riley IIRs */
  19 #define CONVOLVE _ _ _ _
  20
  21 typedef struct {double in, out_low, out_high;} previous_t[N * 2];
  22
  23 typedef struct {
  24   previous_t * previous;
  25   size_t       pos;
  26   double       coefs[3 *(N+1)];
  27 } crossover_t;
  28
  29 static void square_quadratic(char const * name, double const * x, double * y)
  30 {
  31   assert(N == 4);
  32   y[0] = x[0] * x[0];
  33   y[1] = 2 * x[0] * x[1];
  34   y[2] = 2 * x[0] * x[2] + x[1] * x[1];
  35   y[3] = 2 * x[1] * x[2];
  36   y[4] = x[2] * x[2];
  37   lsx_debug("%s=[%.16g %.16g %.16g %.16g %.16g];", name,
  38       y[0], y[1], y[2], y[3], y[4]);
  39 }
  40
  41 static int crossover_setup(sox_effect_t * effp, crossover_t * p, double frequency)
  42 {
  43   double w0 = 2 * M_PI * frequency / effp->in_signal.rate;
  44   double Q = sqrt(.5), alpha = sin(w0)/(2*Q);
  45   double x[9], norm;
  46   int i;
  47
  48   if (w0 > M_PI) {
  49     lsx_fail("frequency must not exceed half the sample-rate (Nyquist rate)");
  50     return SOX_EOF;
  51   }
  52   x[0] =  (1 - cos(w0))/2;           /* Cf. filter_LPF in biquads.c */
  53   x[1] =   1 - cos(w0);
  54   x[2] =  (1 - cos(w0))/2;
  55   x[3] =  (1 + cos(w0))/2;           /* Cf. filter_HPF in biquads.c */
  56   x[4] = -(1 + cos(w0));
  57   x[5] =  (1 + cos(w0))/2;
  58   x[6] =   1 + alpha;
  59   x[7] =  -2*cos(w0);
  60   x[8] =   1 - alpha;
  61   for (norm = x[6], i = 0; i < 9; ++i) x[i] /= norm;
  62   square_quadratic("lb", x    , p->coefs);
  63   square_quadratic("hb", x + 3, p->coefs + 5);
  64   square_quadratic("a" , x + 6, p->coefs + 10);
  65
  66   p->previous = lsx_calloc(effp->in_signal.channels, sizeof(*p->previous));
  67   return SOX_SUCCESS;
  68 }
  69
  70 static int crossover_flow(sox_effect_t * effp, crossover_t * p, sox_sample_t
  71     *ibuf, sox_sample_t *obuf_low, sox_sample_t *obuf_high, size_t len0)
  72 {
  73   double out_low, out_high;
  74   size_t c, len = len0 / effp->in_signal.channels;
  75   assert(len * effp->in_signal.channels == len0);
  76
  77   while (len--) {
  78     p->pos = p->pos? p->pos - 1 : N - 1;
  79     for (c = 0; c < effp->in_signal.channels; ++c) {
  80 #define _ out_low += p->coefs[j] * p->previous[c][p->pos + j].in \
  81         - p->coefs[2*N+2 + j] * p->previous[c][p->pos + j].out_low, ++j;
  82       {
  83         int j = 1;
  84         out_low = p->coefs[0] * *ibuf;
  85         CONVOLVE
  86         assert(j == N+1);
  87         *obuf_low++ = SOX_ROUND_CLIP_COUNT(out_low, effp->clips);
  88       }
  89 #undef _
  90 #define _ out_high += p->coefs[j+N+1] * p->previous[c][p->pos + j].in \
  91         - p->coefs[2*N+2 + j] * p->previous[c][p->pos + j].out_high, ++j;
  92       {
  93         int j = 1;
  94         out_high = p->coefs[N+1] * *ibuf;
  95         CONVOLVE
  96         assert(j == N+1);
  97         *obuf_high++ = SOX_ROUND_CLIP_COUNT(out_high, effp->clips);
  98       }
  99       p->previous[c][p->pos + N].in = p->previous[c][p->pos].in = *ibuf++;
 100       p->previous[c][p->pos + N].out_low = p->previous[c][p->pos].out_low = out_low;
 101       p->previous[c][p->pos + N].out_high = p->previous[c][p->pos].out_high = out_high;
 102     }
 103   }
 104   return SOX_SUCCESS;
 105 }
 106