core/mixer/mixer_c.cpp

   1 #include "config.h"
   2
   3 #include <algorithm>
   4 #include <array>
   5 #include <cstddef>
   6 #include <limits>
   7 #include <variant>
   8
   9 #include "alnumeric.h"
  10 #include "alspan.h"
  11 #include "core/bsinc_defs.h"
  12 #include "core/bufferline.h"
  13 #include "core/cubic_defs.h"
  14 #include "core/mixer/hrtfdefs.h"
  15 #include "core/resampler_limits.h"
  16 #include "defs.h"
  17 #include "hrtfbase.h"
  18 #include "opthelpers.h"
  19
  20 struct CTag;
  21 struct PointTag;
  22 struct LerpTag;
  23 struct CubicTag;
  24 struct BSincTag;
  25 struct FastBSincTag;
  26
  27
  28 namespace {
  29
  30 constexpr uint BsincPhaseDiffBits{MixerFracBits - BSincPhaseBits};
  31 constexpr uint BsincPhaseDiffOne{1 << BsincPhaseDiffBits};
  32 constexpr uint BsincPhaseDiffMask{BsincPhaseDiffOne - 1u};
  33
  34 constexpr uint CubicPhaseDiffBits{MixerFracBits - CubicPhaseBits};
  35 constexpr uint CubicPhaseDiffOne{1 << CubicPhaseDiffBits};
  36 constexpr uint CubicPhaseDiffMask{CubicPhaseDiffOne - 1u};
  37
  38 using SamplerNST = float(const al::span<const float>, const size_t, const uint) noexcept;
  39
  40 template<typename T>
  41 using SamplerT = float(const T&,const al::span<const float>,const size_t,const uint) noexcept;
  42
  43 [[nodiscard]] constexpr
  44 auto do_point(const al::span<const float> vals, const size_t pos, const uint) noexcept -> float
  45 { return vals[pos]; }
  46 [[nodiscard]] constexpr
  47 auto do_lerp(const al::span<const float> vals, const size_t pos, const uint frac) noexcept -> float
  48 { return lerpf(vals[pos+0], vals[pos+1], static_cast<float>(frac)*(1.0f/MixerFracOne)); }
  49 [[nodiscard]] constexpr
  50 auto do_cubic(const CubicState &istate, const al::span<const float> vals, const size_t pos,
  51     const uint frac) noexcept -> float
  52 {
  53     /* Calculate the phase index and factor. */
  54     const uint pi{frac >> CubicPhaseDiffBits}; ASSUME(pi < CubicPhaseCount);
  55     const float pf{static_cast<float>(frac&CubicPhaseDiffMask) * (1.0f/CubicPhaseDiffOne)};
  56
  57     const auto fil = al::span{istate.filter[pi].mCoeffs};
  58     const auto phd = al::span{istate.filter[pi].mDeltas};
  59
  60     /* Apply the phase interpolated filter. */
  61     return (fil[0] + pf*phd[0])*vals[pos+0] + (fil[1] + pf*phd[1])*vals[pos+1]
  62         + (fil[2] + pf*phd[2])*vals[pos+2] + (fil[3] + pf*phd[3])*vals[pos+3];
  63 }
  64 [[nodiscard]] constexpr
  65 auto do_fastbsinc(const BsincState &bsinc, const al::span<const float> vals, const size_t pos,
  66     const uint frac) noexcept -> float
  67 {
  68     const size_t m{bsinc.m};
  69     ASSUME(m > 0);
  70     ASSUME(m <= MaxResamplerPadding);
  71
  72     /* Calculate the phase index and factor. */
  73     const uint pi{frac >> BsincPhaseDiffBits}; ASSUME(pi < BSincPhaseCount);
  74     const float pf{static_cast<float>(frac&BsincPhaseDiffMask) * (1.0f/BsincPhaseDiffOne)};
  75
  76     const auto fil = bsinc.filter.subspan(2_uz*pi*m);
  77     const auto phd = fil.subspan(m);
  78
  79     /* Apply the phase interpolated filter. */
  80     float r{0.0f};
  81     for(size_t j_f{0};j_f < m;++j_f)
  82         r += (fil[j_f] + pf*phd[j_f]) * vals[pos+j_f];
  83     return r;
  84 }
  85 [[nodiscard]] constexpr
  86 auto do_bsinc(const BsincState &bsinc, const al::span<const float> vals, const size_t pos,
  87     const uint frac) noexcept -> float
  88 {
  89     const size_t m{bsinc.m};
  90     ASSUME(m > 0);
  91     ASSUME(m <= MaxResamplerPadding);
  92
  93     /* Calculate the phase index and factor. */
  94     const uint pi{frac >> BsincPhaseDiffBits}; ASSUME(pi < BSincPhaseCount);
  95     const float pf{static_cast<float>(frac&BsincPhaseDiffMask) * (1.0f/BsincPhaseDiffOne)};
  96
  97     const auto fil = bsinc.filter.subspan(2_uz*pi*m);
  98     const auto phd = fil.subspan(m);
  99     const auto scd = fil.subspan(BSincPhaseCount*2_uz*m);
 100     const auto spd = scd.subspan(m);
 101
 102     /* Apply the scale and phase interpolated filter. */
 103     float r{0.0f};
 104     for(size_t j_f{0};j_f < m;++j_f)
 105         r += (fil[j_f] + bsinc.sf*scd[j_f] + pf*(phd[j_f] + bsinc.sf*spd[j_f])) * vals[pos+j_f];
 106     return r;
 107 }
 108
 109 template<SamplerNST Sampler>
 110 void DoResample(const al::span<const float> src, uint frac, const uint increment,
 111     const al::span<float> dst)
 112 {
 113     ASSUME(frac < MixerFracOne);
 114     size_t pos{0};
 115     std::generate(dst.begin(), dst.end(), [&pos,&frac,src,increment]() -> float
 116     {
 117         const float output{Sampler(src, pos, frac)};
 118         frac += increment;
 119         pos  += frac>>MixerFracBits;
 120         frac &= MixerFracMask;
 121         return output;
 122     });
 123 }
 124
 125 template<typename U, SamplerT<U> Sampler>
 126 void DoResample(const U istate, const al::span<const float> src, uint frac, const uint increment,
 127     const al::span<float> dst)
 128 {
 129     ASSUME(frac < MixerFracOne);
 130     size_t pos{0};
 131     std::generate(dst.begin(), dst.end(), [istate,src,&pos,&frac,increment]() -> float
 132     {
 133         const float output{Sampler(istate, src, pos, frac)};
 134         frac += increment;
 135         pos  += frac>>MixerFracBits;
 136         frac &= MixerFracMask;
 137         return output;
 138     });
 139 }
 140
 141 inline void ApplyCoeffs(const al::span<float2> Values, const size_t IrSize,
 142     const ConstHrirSpan Coeffs, const float left, const float right) noexcept
 143 {
 144     ASSUME(IrSize >= MinIrLength);
 145     ASSUME(IrSize <= HrirLength);
 146
 147     auto mix_impulse = [left,right](const float2 &value, const float2 &coeff) noexcept -> float2
 148     { return float2{{value[0] + coeff[0]*left, value[1] + coeff[1]*right}}; };
 149     std::transform(Values.cbegin(), Values.cbegin()+ptrdiff_t(IrSize), Coeffs.cbegin(),
 150         Values.begin(), mix_impulse);
 151 }
 152
 153 force_inline void MixLine(al::span<const float> InSamples, const al::span<float> dst,
 154     float &CurrentGain, const float TargetGain, const float delta, const size_t fade_len,
 155     size_t Counter)
 156 {
 157     const float step{(TargetGain-CurrentGain) * delta};
 158
 159     auto output = dst.begin();
 160     if(std::abs(step) > std::numeric_limits<float>::epsilon())
 161     {
 162         auto input = InSamples.first(fade_len);
 163         InSamples = InSamples.subspan(fade_len);
 164
 165         const float gain{CurrentGain};
 166         float step_count{0.0f};
 167         output = std::transform(input.begin(), input.end(), output, output,
 168             [gain,step,&step_count](const float in, float out) noexcept -> float
 169             {
 170                 out += in * (gain + step*step_count);
 171                 step_count += 1.0f;
 172                 return out;
 173             });
 174
 175         if(fade_len < Counter)
 176         {
 177             CurrentGain = gain + step*step_count;
 178             return;
 179         }
 180     }
 181     CurrentGain = TargetGain;
 182
 183     if(!(std::abs(TargetGain) > GainSilenceThreshold))
 184         return;
 185
 186     std::transform(InSamples.begin(), InSamples.end(), output, output,
 187         [TargetGain](const float in, const float out) noexcept -> float
 188         { return out + in*TargetGain; });
 189 }
 190
 191 } // namespace
 192
 193 template<>
 194 void Resample_<PointTag,CTag>(const InterpState*, const al::span<const float> src, uint frac,
 195     const uint increment, const al::span<float> dst)
 196 { DoResample<do_point>(src.subspan(MaxResamplerEdge), frac, increment, dst); }
 197
 198 template<>
 199 void Resample_<LerpTag,CTag>(const InterpState*, const al::span<const float> src, uint frac,
 200     const uint increment, const al::span<float> dst)
 201 { DoResample<do_lerp>(src.subspan(MaxResamplerEdge), frac, increment, dst); }
 202
 203 template<>
 204 void Resample_<CubicTag,CTag>(const InterpState *state, const al::span<const float> src, uint frac,
 205     const uint increment, const al::span<float> dst)
 206 {
 207     DoResample<CubicState,do_cubic>(std::get<CubicState>(*state), src.subspan(MaxResamplerEdge-1),
 208         frac, increment, dst);
 209 }
 210
 211 template<>
 212 void Resample_<FastBSincTag,CTag>(const InterpState *state, const al::span<const float> src,
 213     uint frac, const uint increment, const al::span<float> dst)
 214 {
 215     const auto istate = std::get<BsincState>(*state);
 216     ASSUME(istate.l <= MaxResamplerEdge);
 217     DoResample<BsincState,do_fastbsinc>(istate, src.subspan(MaxResamplerEdge-istate.l), frac,
 218         increment, dst);
 219 }
 220
 221 template<>
 222 void Resample_<BSincTag,CTag>(const InterpState *state, const al::span<const float> src, uint frac,
 223     const uint increment, const al::span<float> dst)
 224 {
 225     const auto istate = std::get<BsincState>(*state);
 226     ASSUME(istate.l <= MaxResamplerEdge);
 227     DoResample<BsincState,do_bsinc>(istate, src.subspan(MaxResamplerEdge-istate.l), frac,
 228         increment, dst);
 229 }
 230
 231
 232 template<>
 233 void MixHrtf_<CTag>(const al::span<const float> InSamples, const al::span<float2> AccumSamples,
 234     const uint IrSize, const MixHrtfFilter *hrtfparams, const size_t SamplesToDo)
 235 { MixHrtfBase<ApplyCoeffs>(InSamples, AccumSamples, IrSize, hrtfparams, SamplesToDo); }
 236
 237 template<>
 238 void MixHrtfBlend_<CTag>(const al::span<const float> InSamples,const al::span<float2> AccumSamples,
 239     const uint IrSize, const HrtfFilter *oldparams, const MixHrtfFilter *newparams,
 240     const size_t SamplesToDo)
 241 {
 242     MixHrtfBlendBase<ApplyCoeffs>(InSamples, AccumSamples, IrSize, oldparams, newparams,
 243         SamplesToDo);
 244 }
 245
 246 template<>
 247 void MixDirectHrtf_<CTag>(const FloatBufferSpan LeftOut, const FloatBufferSpan RightOut,
 248     const al::span<const FloatBufferLine> InSamples, const al::span<float2> AccumSamples,
 249     const al::span<float,BufferLineSize> TempBuf, const al::span<HrtfChannelState> ChanState,
 250     const size_t IrSize, const size_t SamplesToDo)
 251 {
 252     MixDirectHrtfBase<ApplyCoeffs>(LeftOut, RightOut, InSamples, AccumSamples, TempBuf, ChanState,
 253         IrSize, SamplesToDo);
 254 }
 255
 256
 257 template<>
 258 void Mix_<CTag>(const al::span<const float> InSamples, const al::span<FloatBufferLine> OutBuffer,
 259     const al::span<float> CurrentGains, const al::span<const float> TargetGains,
 260     const size_t Counter, const size_t OutPos)
 261 {
 262     const float delta{(Counter > 0) ? 1.0f / static_cast<float>(Counter) : 0.0f};
 263     const auto fade_len = std::min(Counter, InSamples.size());
 264
 265     auto curgains = CurrentGains.begin();
 266     auto targetgains = TargetGains.cbegin();
 267     for(FloatBufferLine &output : OutBuffer)
 268         MixLine(InSamples, al::span{output}.subspan(OutPos), *curgains++, *targetgains++, delta,
 269             fade_len, Counter);
 270 }
 271
 272 template<>
 273 void Mix_<CTag>(const al::span<const float> InSamples, const al::span<float> OutBuffer,
 274     float &CurrentGain, const float TargetGain, const size_t Counter)
 275 {
 276     const float delta{(Counter > 0) ? 1.0f / static_cast<float>(Counter) : 0.0f};
 277     const auto fade_len = std::min(Counter, InSamples.size());
 278
 279     MixLine(InSamples, OutBuffer, CurrentGain, TargetGain, delta, fade_len, Counter);
 280 }