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