1 files changed, 130 insertions, 6 deletions

diff --git a/alc/bformatdec.cpp b/alc/bformatdec.cpp
index 41710332..b2a2aec9 100644
--- a/alc/bformatdec.cpp
+++ b/alc/bformatdec.cpp
@@ -16,6 +16,8 @@
 #include "alu.h"
 #include "ambdec.h"
 #include "filters/splitter.h"
+#include "front_stablizer.h"
+#include "math_defs.h"
 #include "opthelpers.h"
 
 
@@ -50,10 +52,11 @@ inline auto GetAmbiScales(AmbDecScale scaletype) noexcept
 
 
 BFormatDec::BFormatDec(const AmbDecConf *conf, const bool allow_2band, const size_t inchans,
-    const ALuint srate, const ALuint (&chanmap)[MAX_OUTPUT_CHANNELS]) : mChannelDec{inchans}
+    const ALuint srate, const ALuint (&chanmap)[MAX_OUTPUT_CHANNELS],
+    std::unique_ptr<FrontStablizer> stablizer)
+    : mStablizer{std::move(stablizer)}, mDualBand{allow_2band && (conf->FreqBands == 2)}
+    , mChannelDec{inchans}
 {
-    mDualBand = allow_2band && (conf->FreqBands == 2);
-
     const bool periphonic{(conf->ChanMask&AMBI_PERIPHONIC_MASK) != 0};
     const std::array<float,MAX_AMBI_CHANNELS> &coeff_scale = GetAmbiScales(conf->CoeffScale);
 
@@ -99,10 +102,9 @@ BFormatDec::BFormatDec(const AmbDecConf *conf, const bool allow_2band, const siz
 }
 
 BFormatDec::BFormatDec(const size_t inchans, const al::span<const ChannelDec> coeffs,
-    const al::span<const ChannelDec> coeffslf) : mChannelDec{inchans}
+    const al::span<const ChannelDec> coeffslf, std::unique_ptr<FrontStablizer> stablizer)
+    : mStablizer{std::move(stablizer)}, mDualBand{!coeffslf.empty()}, mChannelDec{inchans}
 {
-    mDualBand = !coeffslf.empty();
-
     if(!mDualBand)
     {
         for(size_t j{0};j < mChannelDec.size();++j)
@@ -159,6 +161,113 @@ void BFormatDec::process(const al::span<FloatBufferLine> OutBuffer,
     }
 }
 
+void BFormatDec::processStablize(const al::span<FloatBufferLine> OutBuffer,
+    const FloatBufferLine *InSamples, const size_t lidx, const size_t ridx, const size_t cidx,
+    const size_t SamplesToDo)
+{
+    ASSUME(SamplesToDo > 0);
+
+    /* Move the existing direct L/R signal out so it doesn't get processed by
+     * the stablizer. Add a delay to it so it stays aligned with the stablizer
+     * delay.
+     */
+    float *RESTRICT mid{al::assume_aligned<16>(mStablizer->MidDirect.data())};
+    float *RESTRICT side{al::assume_aligned<16>(mStablizer->Side.data())};
+    for(size_t i{0};i < SamplesToDo;++i)
+    {
+        mid[FrontStablizer::DelayLength+i] = OutBuffer[lidx][i] + OutBuffer[ridx][i];
+        side[FrontStablizer::DelayLength+i] = OutBuffer[lidx][i] - OutBuffer[ridx][i];
+    }
+    std::fill_n(OutBuffer[lidx].begin(), SamplesToDo, 0.0f);
+    std::fill_n(OutBuffer[ridx].begin(), SamplesToDo, 0.0f);
+
+    /* Decode the B-Format input to OutBuffer. */
+    process(OutBuffer, InSamples, SamplesToDo);
+
+    /* Apply a delay to all channels, except the front-left and front-right, so
+     * they maintain correct timing.
+     */
+    const size_t NumChannels{OutBuffer.size()};
+    for(size_t i{0u};i < NumChannels;i++)
+    {
+        if(i == lidx || i == ridx)
+            continue;
+
+        auto &DelayBuf = mStablizer->DelayBuf[i];
+        auto buffer_end = OutBuffer[i].begin() + SamplesToDo;
+        if LIKELY(SamplesToDo >= FrontStablizer::DelayLength)
+        {
+            auto delay_end = std::rotate(OutBuffer[i].begin(),
+                buffer_end - FrontStablizer::DelayLength, buffer_end);
+            std::swap_ranges(OutBuffer[i].begin(), delay_end, DelayBuf.begin());
+        }
+        else
+        {
+            auto delay_start = std::swap_ranges(OutBuffer[i].begin(), buffer_end,
+                DelayBuf.begin());
+            std::rotate(DelayBuf.begin(), delay_start, DelayBuf.end());
+        }
+    }
+
+    /* Include the side signal for what was just decoded. */
+    for(size_t i{0};i < SamplesToDo;++i)
+        side[FrontStablizer::DelayLength+i] += OutBuffer[lidx][i] - OutBuffer[ridx][i];
+
+    /* Combine the delayed mid signal with the decoded mid signal. Note that
+     * the samples are stored and combined in reverse, so the newest samples
+     * are at the front and the oldest at the back.
+     */
+    al::span<float> tmpbuf{mStablizer->TempBuf.data(), SamplesToDo+FrontStablizer::DelayLength};
+    auto tmpiter = tmpbuf.begin() + SamplesToDo;
+    std::copy(mStablizer->MidDelay.cbegin(), mStablizer->MidDelay.cend(), tmpiter);
+    for(size_t i{0};i < SamplesToDo;++i)
+        *--tmpiter = OutBuffer[lidx][i] + OutBuffer[ridx][i];
+    /* Save the newest samples for next time. */
+    std::copy_n(tmpbuf.cbegin(), mStablizer->MidDelay.size(), mStablizer->MidDelay.begin());
+
+    /* Apply an all-pass on the reversed signal, then reverse the samples to
+     * get the forward signal with a reversed phase shift. The future samples
+     * are included with the all-pass to reduce the error in the output
+     * samples (the smaller the delay, the more error is introduced).
+     */
+    mStablizer->MidFilter.applyAllpass(tmpbuf);
+    tmpbuf = tmpbuf.subspan<FrontStablizer::DelayLength>();
+    std::reverse(tmpbuf.begin(), tmpbuf.end());
+
+    /* Now apply the band-splitter, combining its phase shift with the reversed
+     * phase shift, restoring the original phase on the split signal.
+     */
+    mStablizer->MidFilter.process(tmpbuf, mStablizer->MidHF.data(), mStablizer->MidLF.data());
+
+    /* This pans the separate low- and high-frequency signals between being on
+     * the center channel and the left+right channels. The low-frequency signal
+     * is panned 1/3rd toward center and the high-frequency signal is panned
+     * 1/4th toward center. These values can be tweaked.
+     */
+    const float cos_lf{std::cos(1.0f/3.0f * (al::MathDefs<float>::Pi()*0.5f))};
+    const float cos_hf{std::cos(1.0f/4.0f * (al::MathDefs<float>::Pi()*0.5f))};
+    const float sin_lf{std::sin(1.0f/3.0f * (al::MathDefs<float>::Pi()*0.5f))};
+    const float sin_hf{std::sin(1.0f/4.0f * (al::MathDefs<float>::Pi()*0.5f))};
+    for(size_t i{0};i < SamplesToDo;i++)
+    {
+        const float m{mStablizer->MidLF[i]*cos_lf + mStablizer->MidHF[i]*cos_hf + mid[i]};
+        const float c{mStablizer->MidLF[i]*sin_lf + mStablizer->MidHF[i]*sin_hf};
+        const float s{side[i]};
+
+        /* The generated center channel signal adds to the existing signal,
+         * while the modified left and right channels replace.
+         */
+        OutBuffer[lidx][i] = (m + s) * 0.5f;
+        OutBuffer[ridx][i] = (m - s) * 0.5f;
+        OutBuffer[cidx][i] += c * 0.5f;
+    }
+    /* Move the delayed mid/side samples to the front for next time. */
+    auto mid_end = mStablizer->MidDirect.cbegin() + SamplesToDo;
+    std::copy(mid_end, mid_end+FrontStablizer::DelayLength, mStablizer->MidDirect.begin());
+    auto side_end = mStablizer->Side.cbegin() + SamplesToDo;
+    std::copy(side_end, side_end+FrontStablizer::DelayLength, mStablizer->Side.begin());
+}
+
 
 auto BFormatDec::GetHFOrderScales(const ALuint in_order, const ALuint out_order) noexcept
     -> std::array<float,MAX_AMBI_ORDER+1>
@@ -175,3 +284,18 @@ auto BFormatDec::GetHFOrderScales(const ALuint in_order, const ALuint out_order)
 
     return ret;
 }
+
+std::unique_ptr<BFormatDec> BFormatDec::Create(const AmbDecConf *conf, const bool allow_2band,
+    const size_t inchans, const ALuint srate, const ALuint (&chanmap)[MAX_OUTPUT_CHANNELS],
+    std::unique_ptr<FrontStablizer> stablizer)
+{
+    return std::unique_ptr<BFormatDec>{new(FamCount(inchans))
+        BFormatDec{conf, allow_2band, inchans, srate, chanmap, std::move(stablizer)}};
+}
+std::unique_ptr<BFormatDec> BFormatDec::Create(const size_t inchans,
+    const al::span<const ChannelDec> coeffs, const al::span<const ChannelDec> coeffslf,
+    std::unique_ptr<FrontStablizer> stablizer)
+{
+    return std::unique_ptr<BFormatDec>{new(FamCount(inchans))
+        BFormatDec{inchans, coeffs, coeffslf, std::move(stablizer)}};
+}