core/bs2b.cpp

   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 <algorithm>
  27 #include <cmath>
  28 #include <iterator>
  29
  30 #include "alnumbers.h"
  31 #include "bs2b.h"
  32
  33
  34 /* Set up all data. */
  35 static void init(struct bs2b *bs2b)
  36 {
  37     float Fc_lo, Fc_hi;
  38     float G_lo, G_hi;
  39     float x, g;
  40
  41     switch(bs2b->level)
  42     {
  43     case BS2B_LOW_CLEVEL: /* Low crossfeed level */
  44         Fc_lo = 360.0f;
  45         Fc_hi = 501.0f;
  46         G_lo  = 0.398107170553497f;
  47         G_hi  = 0.205671765275719f;
  48         break;
  49
  50     case BS2B_MIDDLE_CLEVEL: /* Middle crossfeed level */
  51         Fc_lo = 500.0f;
  52         Fc_hi = 711.0f;
  53         G_lo  = 0.459726988530872f;
  54         G_hi  = 0.228208484414988f;
  55         break;
  56
  57     case BS2B_HIGH_CLEVEL: /* High crossfeed level (virtual speakers are closer to itself) */
  58         Fc_lo = 700.0f;
  59         Fc_hi = 1021.0f;
  60         G_lo  = 0.530884444230988f;
  61         G_hi  = 0.250105790667544f;
  62         break;
  63
  64     case BS2B_LOW_ECLEVEL: /* Low easy crossfeed level */
  65         Fc_lo = 360.0f;
  66         Fc_hi = 494.0f;
  67         G_lo  = 0.316227766016838f;
  68         G_hi  = 0.168236228897329f;
  69         break;
  70
  71     case BS2B_MIDDLE_ECLEVEL: /* Middle easy crossfeed level */
  72         Fc_lo = 500.0f;
  73         Fc_hi = 689.0f;
  74         G_lo  = 0.354813389233575f;
  75         G_hi  = 0.187169483835901f;
  76         break;
  77
  78     default: /* High easy crossfeed level */
  79         bs2b->level = BS2B_HIGH_ECLEVEL;
  80
  81         Fc_lo = 700.0f;
  82         Fc_hi = 975.0f;
  83         G_lo  = 0.398107170553497f;
  84         G_hi  = 0.205671765275719f;
  85         break;
  86     } /* switch */
  87
  88     g = 1.0f / (1.0f - G_hi + G_lo);
  89
  90     /* $fc = $Fc / $s;
  91      * $d  = 1 / 2 / pi / $fc;
  92      * $x  = exp(-1 / $d);
  93      */
  94     x           = std::exp(-al::numbers::pi_v<float>*2.0f*Fc_lo/static_cast<float>(bs2b->srate));
  95     bs2b->b1_lo = x;
  96     bs2b->a0_lo = G_lo * (1.0f - x) * g;
  97
  98     x           = std::exp(-al::numbers::pi_v<float>*2.0f*Fc_hi/static_cast<float>(bs2b->srate));
  99     bs2b->b1_hi = x;
 100     bs2b->a0_hi = (1.0f - G_hi * (1.0f - x)) * g;
 101     bs2b->a1_hi = -x * g;
 102 } /* init */
 103
 104
 105 /* Exported functions.
 106  * See descriptions in "bs2b.h"
 107  */
 108
 109 void bs2b_set_params(struct bs2b *bs2b, int level, int srate)
 110 {
 111     if(srate <= 0) srate = 1;
 112
 113     bs2b->level = level;
 114     bs2b->srate = srate;
 115     init(bs2b);
 116 } /* bs2b_set_params */
 117
 118 int bs2b_get_level(struct bs2b *bs2b)
 119 {
 120     return bs2b->level;
 121 } /* bs2b_get_level */
 122
 123 int bs2b_get_srate(struct bs2b *bs2b)
 124 {
 125     return bs2b->srate;
 126 } /* bs2b_get_srate */
 127
 128 void bs2b_clear(struct bs2b *bs2b)
 129 {
 130     std::fill(std::begin(bs2b->history), std::end(bs2b->history), bs2b::t_last_sample{});
 131 } /* bs2b_clear */
 132
 133 void bs2b_cross_feed(struct bs2b *bs2b, float *Left, float *Right, size_t SamplesToDo)
 134 {
 135     const float a0_lo{bs2b->a0_lo};
 136     const float b1_lo{bs2b->b1_lo};
 137     const float a0_hi{bs2b->a0_hi};
 138     const float a1_hi{bs2b->a1_hi};
 139     const float b1_hi{bs2b->b1_hi};
 140     float lsamples[128][2];
 141     float rsamples[128][2];
 142
 143     for(size_t base{0};base < SamplesToDo;)
 144     {
 145         const size_t todo{std::min<size_t>(128, SamplesToDo-base)};
 146
 147         /* Process left input */
 148         float z_lo{bs2b->history[0].lo};
 149         float z_hi{bs2b->history[0].hi};
 150         for(size_t i{0};i < todo;i++)
 151         {
 152             lsamples[i][0] = a0_lo*Left[i] + z_lo;
 153             z_lo = b1_lo*lsamples[i][0];
 154
 155             lsamples[i][1] = a0_hi*Left[i] + z_hi;
 156             z_hi = a1_hi*Left[i] + b1_hi*lsamples[i][1];
 157         }
 158         bs2b->history[0].lo = z_lo;
 159         bs2b->history[0].hi = z_hi;
 160
 161         /* Process right input */
 162         z_lo = bs2b->history[1].lo;
 163         z_hi = bs2b->history[1].hi;
 164         for(size_t i{0};i < todo;i++)
 165         {
 166             rsamples[i][0] = a0_lo*Right[i] + z_lo;
 167             z_lo = b1_lo*rsamples[i][0];
 168
 169             rsamples[i][1] = a0_hi*Right[i] + z_hi;
 170             z_hi = a1_hi*Right[i] + b1_hi*rsamples[i][1];
 171         }
 172         bs2b->history[1].lo = z_lo;
 173         bs2b->history[1].hi = z_hi;
 174
 175         /* Crossfeed */
 176         for(size_t i{0};i < todo;i++)
 177             *(Left++) = lsamples[i][1] + rsamples[i][0];
 178         for(size_t i{0};i < todo;i++)
 179             *(Right++) = rsamples[i][1] + lsamples[i][0];
 180
 181         base += todo;
 182     }
 183 } /* bs2b_cross_feed */