Alc/bs2b.c

   1 /*-
   2  * Copyright (c) 2005 Boris Mikhaylov
   3  *
   4  * Permission is hereby granted, free of charge, to any person obtaining
   5  * a copy of this software and associated documentation files (the
   6  * "Software"), to deal in the Software without restriction, including
   7  * without limitation the rights to use, copy, modify, merge, publish,
   8  * distribute, sublicense, and/or sell copies of the Software, and to
   9  * permit persons to whom the Software is furnished to do so, subject to
  10  * the following conditions:
  11  *
  12  * The above copyright notice and this permission notice shall be
  13  * included in all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
  18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
  19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
  20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
  21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  22  */
  23
  24 #include "config.h"
  25
  26 #include <math.h>
  27 #include <string.h>
  28
  29 #include "bs2b.h"
  30 #include "alu.h"
  31
  32
  33 /* Set up all data. */
  34 static void init(struct bs2b *bs2b)
  35 {
  36     float Fc_lo, Fc_hi;
  37     float G_lo,  G_hi;
  38     float x, g;
  39
  40     switch(bs2b->level)
  41     {
  42     case BS2B_LOW_CLEVEL: /* Low crossfeed level */
  43         Fc_lo = 360.0f;
  44         Fc_hi = 501.0f;
  45         G_lo  = 0.398107170553497f;
  46         G_hi  = 0.205671765275719f;
  47         break;
  48
  49     case BS2B_MIDDLE_CLEVEL: /* Middle crossfeed level */
  50         Fc_lo = 500.0f;
  51         Fc_hi = 711.0f;
  52         G_lo  = 0.459726988530872f;
  53         G_hi  = 0.228208484414988f;
  54         break;
  55
  56     case BS2B_HIGH_CLEVEL: /* High crossfeed level (virtual speakers are closer to itself) */
  57         Fc_lo = 700.0f;
  58         Fc_hi = 1021.0f;
  59         G_lo  = 0.530884444230988f;
  60         G_hi  = 0.250105790667544f;
  61         break;
  62
  63     case BS2B_LOW_ECLEVEL: /* Low easy crossfeed level */
  64         Fc_lo = 360.0f;
  65         Fc_hi = 494.0f;
  66         G_lo  = 0.316227766016838f;
  67         G_hi  = 0.168236228897329f;
  68         break;
  69
  70     case BS2B_MIDDLE_ECLEVEL: /* Middle easy crossfeed level */
  71         Fc_lo = 500.0f;
  72         Fc_hi = 689.0f;
  73         G_lo  = 0.354813389233575f;
  74         G_hi  = 0.187169483835901f;
  75         break;
  76
  77     default: /* High easy crossfeed level */
  78         bs2b->level = BS2B_HIGH_ECLEVEL;
  79
  80         Fc_lo = 700.0f;
  81         Fc_hi = 975.0f;
  82         G_lo  = 0.398107170553497f;
  83         G_hi  = 0.205671765275719f;
  84         break;
  85     } /* switch */
  86
  87     g = 1.0f / (1.0f - G_hi + G_lo);
  88
  89     /* $fc = $Fc / $s;
  90      * $d  = 1 / 2 / pi / $fc;
  91      * $x  = exp(-1 / $d);
  92      */
  93     x           = expf(-2.0f * F_PI * Fc_lo / bs2b->srate);
  94     bs2b->b1_lo = x;
  95     bs2b->a0_lo = G_lo * (1.0f - x) * g;
  96
  97     x           = expf(-2.0f * F_PI * Fc_hi / bs2b->srate);
  98     bs2b->b1_hi = x;
  99     bs2b->a0_hi = (1.0f - G_hi * (1.0f - x)) * g;
 100     bs2b->a1_hi = -x * g;
 101 } /* init */
 102
 103
 104 /* Exported functions.
 105  * See descriptions in "bs2b.h"
 106  */
 107
 108 void bs2b_set_params(struct bs2b *bs2b, int level, int srate)
 109 {
 110     if(srate <= 0) srate = 1;
 111
 112     bs2b->level = level;
 113     bs2b->srate = srate;
 114     init(bs2b);
 115 } /* bs2b_set_params */
 116
 117 int bs2b_get_level(struct bs2b *bs2b)
 118 {
 119     return bs2b->level;
 120 } /* bs2b_get_level */
 121
 122 int bs2b_get_srate(struct bs2b *bs2b)
 123 {
 124     return bs2b->srate;
 125 } /* bs2b_get_srate */
 126
 127 void bs2b_clear(struct bs2b *bs2b)
 128 {
 129     memset(&bs2b->last_sample, 0, sizeof(bs2b->last_sample));
 130 } /* bs2b_clear */
 131
 132 void bs2b_cross_feed(struct bs2b *bs2b, float *restrict Left, float *restrict Right, int SamplesToDo)
 133 {
 134     float lsamples[128][2];
 135     float rsamples[128][2];
 136     int base;
 137
 138     for(base = 0;base < SamplesToDo;)
 139     {
 140         int todo = mini(128, SamplesToDo-base);
 141         int i;
 142
 143         /* Process left input */
 144         lsamples[0][0] = bs2b->a0_lo*Left[0] +
 145                          bs2b->b1_lo*bs2b->last_sample[0].lo;
 146         lsamples[0][1] = bs2b->a0_hi*Left[0] +
 147                          bs2b->a1_hi*bs2b->last_sample[0].asis +
 148                          bs2b->b1_hi*bs2b->last_sample[0].hi;
 149         for(i = 1;i < todo;i++)
 150         {
 151             lsamples[i][0] = bs2b->a0_lo*Left[i] +
 152                              bs2b->b1_lo*lsamples[i-1][0];
 153             lsamples[i][1] = bs2b->a0_hi*Left[i] +
 154                              bs2b->a1_hi*Left[i-1] +
 155                              bs2b->b1_hi*lsamples[i-1][1];
 156         }
 157         bs2b->last_sample[0].asis = Left[i-1];
 158         bs2b->last_sample[0].lo = lsamples[i-1][0];
 159         bs2b->last_sample[0].hi = lsamples[i-1][1];
 160
 161         /* Process right input */
 162         rsamples[0][0] = bs2b->a0_lo*Right[0] +
 163                          bs2b->b1_lo*bs2b->last_sample[1].lo;
 164         rsamples[0][1] = bs2b->a0_hi*Right[0] +
 165                          bs2b->a1_hi*bs2b->last_sample[1].asis +
 166                          bs2b->b1_hi*bs2b->last_sample[1].hi;
 167         for(i = 1;i < todo;i++)
 168         {
 169             rsamples[i][0] = bs2b->a0_lo*Right[i] +
 170                              bs2b->b1_lo*rsamples[i-1][0];
 171             rsamples[i][1] = bs2b->a0_hi*Right[i] +
 172                              bs2b->a1_hi*Right[i-1] +
 173                              bs2b->b1_hi*rsamples[i-1][1];
 174         }
 175         bs2b->last_sample[1].asis = Right[i-1];
 176         bs2b->last_sample[1].lo = rsamples[i-1][0];
 177         bs2b->last_sample[1].hi = rsamples[i-1][1];
 178
 179         /* Crossfeed */
 180         for(i = 0;i < todo;i++)
 181             *(Left++) = lsamples[i][1] + rsamples[i][0];
 182         for(i = 0;i < todo;i++)
 183             *(Right++) = rsamples[i][1] + lsamples[i][0];
 184
 185         base += todo;
 186     }
 187 } /* bs2b_cross_feed */