4 files changed, 100 insertions, 6 deletions
diff --git a/core/mixer/defs.h b/core/mixer/defs.h
index e8e7be6c..ba304f22 100644
--- a/core/mixer/defs.h
+++ b/core/mixer/defs.h
@@ -6,6 +6,7 @@
 
 #include "alspan.h"
 #include "core/bufferline.h"
+#include "core/resampler_limits.h"
 
 struct HrtfChannelState;
 struct HrtfFilter;
@@ -19,12 +20,6 @@ constexpr int MixerFracBits{12};
 constexpr int MixerFracOne{1 << MixerFracBits};
 constexpr int MixerFracMask{MixerFracOne - 1};
 
-/* Maximum number of samples to pad on the ends of a buffer for resampling.
- * Note that the padding is symmetric (half at the beginning and half at the
- * end)!
- */
-constexpr int MaxResamplerPadding{48};
-
 constexpr float GainSilenceThreshold{0.00001f}; /* -100dB */
 
 
diff --git a/core/resampler_limits.h b/core/resampler_limits.h
new file mode 100644
index 00000000..9d4cefda
--- /dev/null
+++ b/core/resampler_limits.h
@@ -0,0 +1,12 @@
+#ifndef CORE_RESAMPLER_LIMITS_H
+#define CORE_RESAMPLER_LIMITS_H
+
+/* Maximum number of samples to pad on the ends of a buffer for resampling.
+ * Note that the padding is symmetric (half at the beginning and half at the
+ * end)!
+ */
+constexpr int MaxResamplerPadding{48};
+
+constexpr int MaxResamplerEdge{MaxResamplerPadding >> 1};
+
+#endif /* CORE_RESAMPLER_LIMITS_H */
diff --git a/core/uhjfilter.cpp b/core/uhjfilter.cpp
index bfcb83a3..d535522c 100644
--- a/core/uhjfilter.cpp
+++ b/core/uhjfilter.cpp
@@ -14,6 +14,8 @@
 
 namespace {
 
+static_assert(Uhj2Encoder::sFilterDelay==UhjDecoder::sFilterDelay, "UHJ filter delays mismatch");
+
 using complex_d = std::complex<double>;
 
 const PhaseShifterT<Uhj2Encoder::sFilterDelay*2> PShift{};
@@ -90,3 +92,68 @@ void Uhj2Encoder::encode(const FloatBufferSpan LeftOut, const FloatBufferSpan Ri
     std::copy(mS.cbegin()+SamplesToDo, mS.cbegin()+SamplesToDo+sFilterDelay, mS.begin());
     std::copy(mD.cbegin()+SamplesToDo, mD.cbegin()+SamplesToDo+sFilterDelay, mD.begin());
 }
+
+
+/* Decoding UHJ is done as:
+ *
+ * S = Left + Right
+ * D = Left - Right
+ *
+ * W = 0.981530*S + 0.197484*j(0.828347*D + 0.767835*T)
+ * X = 0.418504*S - j(0.828347*D + 0.767835*T)
+ * Y = 0.795954*D - 0.676406*T + j(0.186626*S)
+ * Z = 1.023332*Q
+ *
+ * where j is a +90 degree phase shift. 3-channel UHJ excludes Q, while 2-
+ * channel excludes Q and T. The B-Format signal reconstructed from 2-channel
+ * UHJ should not be run through a normal B-Format decoder, as it needs
+ * different shelf filters.
+ */
+void UhjDecoder::decode(const al::span<float*, 3> Samples, const size_t SamplesToDo,
+    const size_t ForwardSamples)
+{
+    ASSUME(SamplesToDo > 0);
+
+    /* S = Left + Right */
+    for(size_t i{0};i < SamplesToDo+sFilterDelay;++i)
+        mS[i] = Samples[0][i] + Samples[1][i];
+
+    /* D = Left - Right */
+    for(size_t i{0};i < SamplesToDo+sFilterDelay;++i)
+        mD[i] = Samples[0][i] - Samples[1][i];
+
+    /* T */
+    for(size_t i{0};i < SamplesToDo+sFilterDelay;++i)
+        mT[i] = Samples[2][i];
+
+    float *woutput{Samples[0]};
+    float *xoutput{Samples[1]};
+    float *youtput{Samples[2]};
+
+    /* Precompute j(0.828347*D + 0.767835*T) and store in xoutput. */
+    auto tmpiter = std::copy(mDTHistory.cbegin(), mDTHistory.cend(), mTemp.begin());
+    std::transform(mD.cbegin(), mD.cbegin()+SamplesToDo+sFilterDelay, mT.cbegin(), tmpiter,
+        [](const float d, const float t) noexcept { return 0.828347f*d + 0.767835f*t; });
+    std::copy_n(mTemp.cbegin()+ForwardSamples, mDTHistory.size(), mDTHistory.begin());
+    PShift.process({xoutput, SamplesToDo}, mTemp.data());
+
+    for(size_t i{0};i < SamplesToDo;++i)
+    {
+        /* W = 0.981530*S + 0.197484*j(0.828347*D + 0.767835*T) */
+        woutput[i] = 0.981530f*mS[i] + 0.197484f*xoutput[i];
+        /* X = 0.418504*S - j(0.828347*D + 0.767835*T) */
+        xoutput[i] = 0.418504f*mS[i] - xoutput[i];
+    }
+
+    /* Precompute j*S and store in youtput. */
+    tmpiter = std::copy(mSHistory.cbegin(), mSHistory.cend(), mTemp.begin());
+    std::copy_n(mS.cbegin(), SamplesToDo+sFilterDelay, tmpiter);
+    std::copy_n(mTemp.cbegin()+ForwardSamples, mSHistory.size(), mSHistory.begin());
+    PShift.process({youtput, SamplesToDo}, mTemp.data());
+
+    for(size_t i{0};i < SamplesToDo;++i)
+    {
+        /* Y = 0.795954*D - 0.676406*T + j(0.186626*S) */
+        youtput[i] = 0.795954f*mD[i] - 0.676406f*mT[i] + 0.186626f*youtput[i];
+    }
+}
diff --git a/core/uhjfilter.h b/core/uhjfilter.h
index 13beea1e..b07488e5 100644
--- a/core/uhjfilter.h
+++ b/core/uhjfilter.h
@@ -5,6 +5,7 @@
 
 #include "almalloc.h"
 #include "bufferline.h"
+#include "resampler_limits.h"
 
 
 struct Uhj2Encoder {
@@ -32,4 +33,23 @@ struct Uhj2Encoder {
     DEF_NEWDEL(Uhj2Encoder)
 };
 
+
+struct UhjDecoder {
+    constexpr static size_t sFilterDelay{128};
+
+    alignas(16) std::array<float,BufferLineSize+MaxResamplerEdge+sFilterDelay> mS{};
+    alignas(16) std::array<float,BufferLineSize+MaxResamplerEdge+sFilterDelay> mD{};
+    alignas(16) std::array<float,BufferLineSize+MaxResamplerEdge+sFilterDelay> mT{};
+
+    alignas(16) std::array<float,sFilterDelay-1> mDTHistory{};
+    alignas(16) std::array<float,sFilterDelay-1> mSHistory{};
+
+    alignas(16) std::array<float,BufferLineSize+MaxResamplerEdge + sFilterDelay*2> mTemp{};
+
+    void decode(const al::span<float*,3> Samples, const size_t SamplesToDo,
+        const size_t ForwardSamples);
+
+    DEF_NEWDEL(UhjDecoder)
+};
+
 #endif /* CORE_UHJFILTER_H */