core/uhjfilter.cpp

   1
   2 #include "config.h"
   3
   4 #include "uhjfilter.h"
   5
   6 #include <algorithm>
   7 #include <iterator>
   8
   9 #include "alcomplex.h"
  10 #include "alnumeric.h"
  11 #include "opthelpers.h"
  12 #include "phase_shifter.h"
  13
  14
  15 namespace {
  16
  17 const PhaseShifterT<UhjFilterBase::sFilterDelay*2> PShift{};
  18
  19 } // namespace
  20
  21
  22 /* Encoding UHJ from B-Format is done as:
  23  *
  24  * S = 0.9396926*W + 0.1855740*X
  25  * D = j(-0.3420201*W + 0.5098604*X) + 0.6554516*Y
  26  *
  27  * Left = (S + D)/2.0
  28  * Right = (S - D)/2.0
  29  * T = j(-0.1432*W + 0.6512*X) - 0.7071068*Y
  30  * Q = 0.9772*Z
  31  *
  32  * where j is a wide-band +90 degree phase shift. 3-channel UHJ excludes Q,
  33  * while 2-channel excludes Q and T.
  34  *
  35  * The phase shift is done using a linear FIR filter derived from an FFT'd
  36  * impulse with the desired shift.
  37  */
  38
  39 void UhjEncoder::encode(float *LeftOut, float *RightOut, const FloatBufferLine *InSamples,
  40     const size_t SamplesToDo)
  41 {
  42     ASSUME(SamplesToDo > 0);
  43
  44     float *RESTRICT left{al::assume_aligned<16>(LeftOut)};
  45     float *RESTRICT right{al::assume_aligned<16>(RightOut)};
  46
  47     const float *RESTRICT winput{al::assume_aligned<16>(InSamples[0].data())};
  48     const float *RESTRICT xinput{al::assume_aligned<16>(InSamples[1].data())};
  49     const float *RESTRICT yinput{al::assume_aligned<16>(InSamples[2].data())};
  50
  51     /* Combine the previously delayed S/D signal with the input. Include any
  52      * existing direct signal with it.
  53      */
  54
  55     /* S = 0.9396926*W + 0.1855740*X */
  56     auto miditer = mS.begin() + sFilterDelay;
  57     std::transform(winput, winput+SamplesToDo, xinput, miditer,
  58         [](const float w, const float x) noexcept -> float
  59         { return 0.9396926f*w + 0.1855740f*x; });
  60     for(size_t i{0};i < SamplesToDo;++i,++miditer)
  61         *miditer += left[i] + right[i];
  62
  63     /* D = 0.6554516*Y */
  64     auto sideiter = mD.begin() + sFilterDelay;
  65     std::transform(yinput, yinput+SamplesToDo, sideiter,
  66         [](const float y) noexcept -> float { return 0.6554516f*y; });
  67     for(size_t i{0};i < SamplesToDo;++i,++sideiter)
  68         *sideiter += left[i] - right[i];
  69
  70     /* D += j(-0.3420201*W + 0.5098604*X) */
  71     auto tmpiter = std::copy(mWXHistory.cbegin(), mWXHistory.cend(), mTemp.begin());
  72     std::transform(winput, winput+SamplesToDo, xinput, tmpiter,
  73         [](const float w, const float x) noexcept -> float
  74         { return -0.3420201f*w + 0.5098604f*x; });
  75     std::copy_n(mTemp.cbegin()+SamplesToDo, mWXHistory.size(), mWXHistory.begin());
  76     PShift.processAccum({mD.data(), SamplesToDo}, mTemp.data());
  77
  78     /* Left = (S + D)/2.0 */
  79     for(size_t i{0};i < SamplesToDo;i++)
  80         left[i] = (mS[i] + mD[i]) * 0.5f;
  81     /* Right = (S - D)/2.0 */
  82     for(size_t i{0};i < SamplesToDo;i++)
  83         right[i] = (mS[i] - mD[i]) * 0.5f;
  84
  85     /* Copy the future samples to the front for next time. */
  86     std::copy(mS.cbegin()+SamplesToDo, mS.cbegin()+SamplesToDo+sFilterDelay, mS.begin());
  87     std::copy(mD.cbegin()+SamplesToDo, mD.cbegin()+SamplesToDo+sFilterDelay, mD.begin());
  88 }
  89
  90
  91 /* Decoding UHJ is done as:
  92  *
  93  * S = Left + Right
  94  * D = Left - Right
  95  *
  96  * W = 0.981532*S + 0.197484*j(0.828331*D + 0.767820*T)
  97  * X = 0.418496*S - j(0.828331*D + 0.767820*T)
  98  * Y = 0.795968*D - 0.676392*T + j(0.186633*S)
  99  * Z = 1.023332*Q
 100  *
 101  * where j is a +90 degree phase shift. 3-channel UHJ excludes Q, while 2-
 102  * channel excludes Q and T.
 103  */
 104 void UhjDecoder::decode(const al::span<float*> samples, const size_t samplesToDo,
 105     const size_t forwardSamples)
 106 {
 107     ASSUME(samplesToDo > 0);
 108
 109     {
 110         const float *RESTRICT left{al::assume_aligned<16>(samples[0])};
 111         const float *RESTRICT right{al::assume_aligned<16>(samples[1])};
 112         const float *RESTRICT t{al::assume_aligned<16>(samples[2])};
 113
 114         /* S = Left + Right */
 115         for(size_t i{0};i < samplesToDo+sFilterDelay;++i)
 116             mS[i] = left[i] + right[i];
 117
 118         /* D = Left - Right */
 119         for(size_t i{0};i < samplesToDo+sFilterDelay;++i)
 120             mD[i] = left[i] - right[i];
 121
 122         /* T */
 123         for(size_t i{0};i < samplesToDo+sFilterDelay;++i)
 124             mT[i] = t[i];
 125     }
 126
 127     float *RESTRICT woutput{al::assume_aligned<16>(samples[0])};
 128     float *RESTRICT xoutput{al::assume_aligned<16>(samples[1])};
 129     float *RESTRICT youtput{al::assume_aligned<16>(samples[2])};
 130
 131     /* Precompute j(0.828331*D + 0.767820*T) and store in xoutput. */
 132     auto tmpiter = std::copy(mDTHistory.cbegin(), mDTHistory.cend(), mTemp.begin());
 133     std::transform(mD.cbegin(), mD.cbegin()+samplesToDo+sFilterDelay, mT.cbegin(), tmpiter,
 134         [](const float d, const float t) noexcept { return 0.828331f*d + 0.767820f*t; });
 135     std::copy_n(mTemp.cbegin()+forwardSamples, mDTHistory.size(), mDTHistory.begin());
 136     PShift.process({xoutput, samplesToDo}, mTemp.data());
 137
 138     /* W = 0.981532*S + 0.197484*j(0.828331*D + 0.767820*T) */
 139     for(size_t i{0};i < samplesToDo;++i)
 140         woutput[i] = 0.981532f*mS[i] + 0.197484f*xoutput[i];
 141     /* X = 0.418496*S - j(0.828331*D + 0.767820*T) */
 142     for(size_t i{0};i < samplesToDo;++i)
 143         xoutput[i] = 0.418496f*mS[i] - xoutput[i];
 144
 145     /* Precompute j*S and store in youtput. */
 146     tmpiter = std::copy(mSHistory.cbegin(), mSHistory.cend(), mTemp.begin());
 147     std::copy_n(mS.cbegin(), samplesToDo+sFilterDelay, tmpiter);
 148     std::copy_n(mTemp.cbegin()+forwardSamples, mSHistory.size(), mSHistory.begin());
 149     PShift.process({youtput, samplesToDo}, mTemp.data());
 150
 151     /* Y = 0.795968*D - 0.676392*T + j(0.186633*S) */
 152     for(size_t i{0};i < samplesToDo;++i)
 153         youtput[i] = 0.795968f*mD[i] - 0.676392f*mT[i] + 0.186633f*youtput[i];
 154
 155     if(samples.size() > 3)
 156     {
 157         float *RESTRICT zoutput{al::assume_aligned<16>(samples[3])};
 158         /* Z = 1.023332*Q */
 159         for(size_t i{0};i < samplesToDo;++i)
 160             zoutput[i] = 1.023332f*zoutput[i];
 161     }
 162 }
 163
 164
 165 /* Super Stereo processing is done as:
 166  *
 167  * S = Left + Right
 168  * D = Left - Right
 169  *
 170  * W = 0.6098637*S - 0.6896511*j*w*D
 171  * X = 0.8624776*S + 0.7626955*j*w*D
 172  * Y = 1.6822415*w*D - 0.2156194*j*S
 173  *
 174  * where j is a +90 degree phase shift. w is a variable control for the
 175  * resulting stereo width, with the range 0 <= w <= 0.7.
 176  */
 177 void UhjDecoder::decodeStereo(const al::span<float*> samples, const size_t samplesToDo,
 178     const size_t forwardSamples)
 179 {
 180     ASSUME(samplesToDo > 0);
 181
 182     {
 183         const float *RESTRICT left{al::assume_aligned<16>(samples[0])};
 184         const float *RESTRICT right{al::assume_aligned<16>(samples[1])};
 185
 186         for(size_t i{0};i < samplesToDo+sFilterDelay;++i)
 187             mS[i] = left[i] + right[i];
 188
 189         /* Pre-apply the width factor to the difference signal D. Smoothly
 190          * interpolate when it changes.
 191          */
 192         const float wtarget{mWidthControl};
 193         const float wcurrent{unlikely(mCurrentWidth < 0.0f) ? wtarget : mCurrentWidth};
 194         const float wstep{(wtarget - wcurrent) / static_cast<float>(forwardSamples)};
 195         if(likely(wstep < 0.00001f))
 196         {
 197             for(size_t i{0};i < samplesToDo+sFilterDelay;++i)
 198                 mD[i] = (left[i] - right[i]) * wtarget;
 199         }
 200         else
 201         {
 202             float fi{0.0f};
 203             size_t i{0};
 204             for(;i < forwardSamples;++i)
 205             {
 206                 mD[i] = (left[i] - right[i]) * (wcurrent + wstep*fi);
 207                 fi += 1.0f;
 208             }
 209             for(;i < samplesToDo+sFilterDelay;++i)
 210                 mD[i] = (left[i] - right[i]) * wtarget;
 211         }
 212         mCurrentWidth = wtarget;
 213     }
 214
 215     float *RESTRICT woutput{al::assume_aligned<16>(samples[0])};
 216     float *RESTRICT xoutput{al::assume_aligned<16>(samples[1])};
 217     float *RESTRICT youtput{al::assume_aligned<16>(samples[2])};
 218
 219     /* Precompute j*D and store in xoutput. */
 220     auto tmpiter = std::copy(mDTHistory.cbegin(), mDTHistory.cend(), mTemp.begin());
 221     std::copy_n(mD.cbegin(), samplesToDo+sFilterDelay, tmpiter);
 222     std::copy_n(mTemp.cbegin()+forwardSamples, mDTHistory.size(), mDTHistory.begin());
 223     PShift.process({xoutput, samplesToDo}, mTemp.data());
 224
 225     /* W = 0.6098637*S - 0.6896511*j*w*D */
 226     for(size_t i{0};i < samplesToDo;++i)
 227         woutput[i] = 0.6098637f*mS[i] - 0.6896511f*xoutput[i];
 228     /* X = 0.8624776*S + 0.7626955*j*w*D */
 229     for(size_t i{0};i < samplesToDo;++i)
 230         xoutput[i] = 0.8624776f*mS[i] + 0.7626955f*xoutput[i];
 231
 232     /* Precompute j*S and store in youtput. */
 233     tmpiter = std::copy(mSHistory.cbegin(), mSHistory.cend(), mTemp.begin());
 234     std::copy_n(mS.cbegin(), samplesToDo+sFilterDelay, tmpiter);
 235     std::copy_n(mTemp.cbegin()+forwardSamples, mSHistory.size(), mSHistory.begin());
 236     PShift.process({youtput, samplesToDo}, mTemp.data());
 237
 238     /* Y = 1.6822415*w*D - 0.2156194*j*S */
 239     for(size_t i{0};i < samplesToDo;++i)
 240         youtput[i] = 1.6822415f*mD[i] - 0.2156194f*youtput[i];
 241 }