core/mixer/mixer_c.cpp

   1 #include "config.h"
   2
   3 #include <cassert>
   4 #include <cmath>
   5 #include <limits>
   6
   7 #include "alnumeric.h"
   8 #include "core/bsinc_defs.h"
   9 #include "core/cubic_defs.h"
  10 #include "defs.h"
  11 #include "hrtfbase.h"
  12
  13 struct CTag;
  14 struct PointTag;
  15 struct LerpTag;
  16 struct CubicTag;
  17 struct BSincTag;
  18 struct FastBSincTag;
  19
  20
  21 namespace {
  22
  23 constexpr uint BsincPhaseDiffBits{MixerFracBits - BSincPhaseBits};
  24 constexpr uint BsincPhaseDiffOne{1 << BsincPhaseDiffBits};
  25 constexpr uint BsincPhaseDiffMask{BsincPhaseDiffOne - 1u};
  26
  27 constexpr uint CubicPhaseDiffBits{MixerFracBits - CubicPhaseBits};
  28 constexpr uint CubicPhaseDiffOne{1 << CubicPhaseDiffBits};
  29 constexpr uint CubicPhaseDiffMask{CubicPhaseDiffOne - 1u};
  30
  31 inline float do_point(const InterpState&, const float *RESTRICT vals, const uint)
  32 { return vals[0]; }
  33 inline float do_lerp(const InterpState&, const float *RESTRICT vals, const uint frac)
  34 { return lerpf(vals[0], vals[1], static_cast<float>(frac)*(1.0f/MixerFracOne)); }
  35 inline float do_cubic(const InterpState &istate, const float *RESTRICT vals, const uint frac)
  36 {
  37     /* Calculate the phase index and factor. */
  38     const uint pi{frac >> CubicPhaseDiffBits};
  39     const float pf{static_cast<float>(frac&CubicPhaseDiffMask) * (1.0f/CubicPhaseDiffOne)};
  40
  41     const float *RESTRICT fil{al::assume_aligned<16>(istate.cubic.filter[pi].mCoeffs)};
  42     const float *RESTRICT phd{al::assume_aligned<16>(istate.cubic.filter[pi].mDeltas)};
  43
  44     /* Apply the phase interpolated filter. */
  45     return (fil[0] + pf*phd[0])*vals[0] + (fil[1] + pf*phd[1])*vals[1]
  46         + (fil[2] + pf*phd[2])*vals[2] + (fil[3] + pf*phd[3])*vals[3];
  47 }
  48 inline float do_bsinc(const InterpState &istate, const float *RESTRICT vals, const uint frac)
  49 {
  50     const size_t m{istate.bsinc.m};
  51     ASSUME(m > 0);
  52
  53     /* Calculate the phase index and factor. */
  54     const uint pi{frac >> BsincPhaseDiffBits};
  55     const float pf{static_cast<float>(frac&BsincPhaseDiffMask) * (1.0f/BsincPhaseDiffOne)};
  56
  57     const float *RESTRICT fil{istate.bsinc.filter + m*pi*2};
  58     const float *RESTRICT phd{fil + m};
  59     const float *RESTRICT scd{fil + BSincPhaseCount*2*m};
  60     const float *RESTRICT spd{scd + m};
  61
  62     /* Apply the scale and phase interpolated filter. */
  63     float r{0.0f};
  64     for(size_t j_f{0};j_f < m;j_f++)
  65         r += (fil[j_f] + istate.bsinc.sf*scd[j_f] + pf*(phd[j_f] + istate.bsinc.sf*spd[j_f])) * vals[j_f];
  66     return r;
  67 }
  68 inline float do_fastbsinc(const InterpState &istate, const float *RESTRICT vals, const uint frac)
  69 {
  70     const size_t m{istate.bsinc.m};
  71     ASSUME(m > 0);
  72
  73     /* Calculate the phase index and factor. */
  74     const uint pi{frac >> BsincPhaseDiffBits};
  75     const float pf{static_cast<float>(frac&BsincPhaseDiffMask) * (1.0f/BsincPhaseDiffOne)};
  76
  77     const float *RESTRICT fil{istate.bsinc.filter + m*pi*2};
  78     const float *RESTRICT phd{fil + m};
  79
  80     /* Apply the phase interpolated filter. */
  81     float r{0.0f};
  82     for(size_t j_f{0};j_f < m;j_f++)
  83         r += (fil[j_f] + pf*phd[j_f]) * vals[j_f];
  84     return r;
  85 }
  86
  87 using SamplerT = float(&)(const InterpState&, const float*RESTRICT, const uint);
  88 template<SamplerT Sampler>
  89 void DoResample(const InterpState *state, const float *RESTRICT src, uint frac,
  90     const uint increment, const al::span<float> dst)
  91 {
  92     const InterpState istate{*state};
  93     ASSUME(frac < MixerFracOne);
  94     for(float &out : dst)
  95     {
  96         out = Sampler(istate, src, frac);
  97
  98         frac += increment;
  99         src  += frac>>MixerFracBits;
 100         frac &= MixerFracMask;
 101     }
 102 }
 103
 104 inline void ApplyCoeffs(float2 *RESTRICT Values, const size_t IrSize, const ConstHrirSpan Coeffs,
 105     const float left, const float right)
 106 {
 107     ASSUME(IrSize >= MinIrLength);
 108     for(size_t c{0};c < IrSize;++c)
 109     {
 110         Values[c][0] += Coeffs[c][0] * left;
 111         Values[c][1] += Coeffs[c][1] * right;
 112     }
 113 }
 114
 115 force_inline void MixLine(const al::span<const float> InSamples, float *RESTRICT dst,
 116     float &CurrentGain, const float TargetGain, const float delta, const size_t min_len,
 117     size_t Counter)
 118 {
 119     float gain{CurrentGain};
 120     const float step{(TargetGain-gain) * delta};
 121
 122     size_t pos{0};
 123     if(!(std::abs(step) > std::numeric_limits<float>::epsilon()))
 124         gain = TargetGain;
 125     else
 126     {
 127         float step_count{0.0f};
 128         for(;pos != min_len;++pos)
 129         {
 130             dst[pos] += InSamples[pos] * (gain + step*step_count);
 131             step_count += 1.0f;
 132         }
 133         if(pos == Counter)
 134             gain = TargetGain;
 135         else
 136             gain += step*step_count;
 137     }
 138     CurrentGain = gain;
 139
 140     if(!(std::abs(gain) > GainSilenceThreshold))
 141         return;
 142     for(;pos != InSamples.size();++pos)
 143         dst[pos] += InSamples[pos] * gain;
 144 }
 145
 146 } // namespace
 147
 148 template<>
 149 void Resample_<PointTag,CTag>(const InterpState *state, const float *RESTRICT src, uint frac,
 150     const uint increment, const al::span<float> dst)
 151 { DoResample<do_point>(state, src, frac, increment, dst); }
 152
 153 template<>
 154 void Resample_<LerpTag,CTag>(const InterpState *state, const float *RESTRICT src, uint frac,
 155     const uint increment, const al::span<float> dst)
 156 { DoResample<do_lerp>(state, src, frac, increment, dst); }
 157
 158 template<>
 159 void Resample_<CubicTag,CTag>(const InterpState *state, const float *RESTRICT src, uint frac,
 160     const uint increment, const al::span<float> dst)
 161 { DoResample<do_cubic>(state, src-1, frac, increment, dst); }
 162
 163 template<>
 164 void Resample_<BSincTag,CTag>(const InterpState *state, const float *RESTRICT src, uint frac,
 165     const uint increment, const al::span<float> dst)
 166 { DoResample<do_bsinc>(state, src-state->bsinc.l, frac, increment, dst); }
 167
 168 template<>
 169 void Resample_<FastBSincTag,CTag>(const InterpState *state, const float *RESTRICT src, uint frac,
 170     const uint increment, const al::span<float> dst)
 171 { DoResample<do_fastbsinc>(state, src-state->bsinc.l, frac, increment, dst); }
 172
 173
 174 template<>
 175 void MixHrtf_<CTag>(const float *InSamples, float2 *AccumSamples, const uint IrSize,
 176     const MixHrtfFilter *hrtfparams, const size_t BufferSize)
 177 { MixHrtfBase<ApplyCoeffs>(InSamples, AccumSamples, IrSize, hrtfparams, BufferSize); }
 178
 179 template<>
 180 void MixHrtfBlend_<CTag>(const float *InSamples, float2 *AccumSamples, const uint IrSize,
 181     const HrtfFilter *oldparams, const MixHrtfFilter *newparams, const size_t BufferSize)
 182 {
 183     MixHrtfBlendBase<ApplyCoeffs>(InSamples, AccumSamples, IrSize, oldparams, newparams,
 184         BufferSize);
 185 }
 186
 187 template<>
 188 void MixDirectHrtf_<CTag>(const FloatBufferSpan LeftOut, const FloatBufferSpan RightOut,
 189     const al::span<const FloatBufferLine> InSamples, float2 *AccumSamples,
 190     float *TempBuf, HrtfChannelState *ChanState, const size_t IrSize, const size_t BufferSize)
 191 {
 192     MixDirectHrtfBase<ApplyCoeffs>(LeftOut, RightOut, InSamples, AccumSamples, TempBuf, ChanState,
 193         IrSize, BufferSize);
 194 }
 195
 196
 197 template<>
 198 void Mix_<CTag>(const al::span<const float> InSamples, const al::span<FloatBufferLine> OutBuffer,
 199     float *CurrentGains, const float *TargetGains, const size_t Counter, const size_t OutPos)
 200 {
 201     const float delta{(Counter > 0) ? 1.0f / static_cast<float>(Counter) : 0.0f};
 202     const auto min_len = minz(Counter, InSamples.size());
 203
 204     for(FloatBufferLine &output : OutBuffer)
 205         MixLine(InSamples, al::assume_aligned<16>(output.data()+OutPos), *CurrentGains++,
 206             *TargetGains++, delta, min_len, Counter);
 207 }
 208
 209 template<>
 210 void Mix_<CTag>(const al::span<const float> InSamples, float *OutBuffer, float &CurrentGain,
 211     const float TargetGain, const size_t Counter)
 212 {
 213     const float delta{(Counter > 0) ? 1.0f / static_cast<float>(Counter) : 0.0f};
 214     const auto min_len = minz(Counter, InSamples.size());
 215
 216     MixLine(InSamples, al::assume_aligned<16>(OutBuffer), CurrentGain,
 217         TargetGain, delta, min_len, Counter);
 218 }