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