alc/effects/pshifter.cpp

   1 /**
   2  * OpenAL cross platform audio library
   3  * Copyright (C) 2018 by Raul Herraiz.
   4  * This library is free software; you can redistribute it and/or
   5  *  modify it under the terms of the GNU Library General Public
   6  *  License as published by the Free Software Foundation; either
   7  *  version 2 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  *  Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  *  License along with this library; if not, write to the
  16  *  Free Software Foundation, Inc.,
  17  *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18  * Or go to http://www.gnu.org/copyleft/lgpl.html
  19  */
  20
  21 #include "config.h"
  22
  23 #include <algorithm>
  24 #include <array>
  25 #include <cmath>
  26 #include <complex>
  27 #include <cstdlib>
  28 #include <iterator>
  29
  30 #include "alc/effects/base.h"
  31 #include "alcomplex.h"
  32 #include "almalloc.h"
  33 #include "alnumbers.h"
  34 #include "alnumeric.h"
  35 #include "alspan.h"
  36 #include "core/bufferline.h"
  37 #include "core/devformat.h"
  38 #include "core/device.h"
  39 #include "core/effectslot.h"
  40 #include "core/mixer.h"
  41 #include "core/mixer/defs.h"
  42 #include "intrusive_ptr.h"
  43
  44 struct ContextBase;
  45
  46
  47 namespace {
  48
  49 using uint = unsigned int;
  50 using complex_f = std::complex<float>;
  51
  52 constexpr size_t StftSize{1024};
  53 constexpr size_t StftHalfSize{StftSize >> 1};
  54 constexpr size_t OversampleFactor{8};
  55
  56 static_assert(StftSize%OversampleFactor == 0, "Factor must be a clean divisor of the size");
  57 constexpr size_t StftStep{StftSize / OversampleFactor};
  58
  59 /* Define a Hann window, used to filter the STFT input and output. */
  60 struct Windower {
  61     alignas(16) std::array<float,StftSize> mData;
  62
  63     Windower()
  64     {
  65         /* Create lookup table of the Hann window for the desired size. */
  66         for(size_t i{0};i < StftHalfSize;i++)
  67         {
  68             constexpr double scale{al::numbers::pi / double{StftSize}};
  69             const double val{std::sin((static_cast<double>(i)+0.5) * scale)};
  70             mData[i] = mData[StftSize-1-i] = static_cast<float>(val * val);
  71         }
  72     }
  73 };
  74 const Windower gWindow{};
  75
  76
  77 struct FrequencyBin {
  78     float Magnitude;
  79     float FreqBin;
  80 };
  81
  82
  83 struct PshifterState final : public EffectState {
  84     /* Effect parameters */
  85     size_t mCount;
  86     size_t mPos;
  87     uint mPitchShiftI;
  88     float mPitchShift;
  89
  90     /* Effects buffers */
  91     std::array<float,StftSize> mFIFO;
  92     std::array<float,StftHalfSize+1> mLastPhase;
  93     std::array<float,StftHalfSize+1> mSumPhase;
  94     std::array<float,StftSize> mOutputAccum;
  95
  96     std::array<complex_f,StftSize> mFftBuffer;
  97
  98     std::array<FrequencyBin,StftHalfSize+1> mAnalysisBuffer;
  99     std::array<FrequencyBin,StftHalfSize+1> mSynthesisBuffer;
 100
 101     alignas(16) FloatBufferLine mBufferOut;
 102
 103     /* Effect gains for each output channel */
 104     float mCurrentGains[MaxAmbiChannels];
 105     float mTargetGains[MaxAmbiChannels];
 106
 107
 108     void deviceUpdate(const DeviceBase *device, const BufferStorage *buffer) override;
 109     void update(const ContextBase *context, const EffectSlot *slot, const EffectProps *props,
 110         const EffectTarget target) override;
 111     void process(const size_t samplesToDo, const al::span<const FloatBufferLine> samplesIn,
 112         const al::span<FloatBufferLine> samplesOut) override;
 113
 114     DEF_NEWDEL(PshifterState)
 115 };
 116
 117 void PshifterState::deviceUpdate(const DeviceBase*, const BufferStorage*)
 118 {
 119     /* (Re-)initializing parameters and clear the buffers. */
 120     mCount       = 0;
 121     mPos         = StftSize - StftStep;
 122     mPitchShiftI = MixerFracOne;
 123     mPitchShift  = 1.0f;
 124
 125     mFIFO.fill(0.0f);
 126     mLastPhase.fill(0.0f);
 127     mSumPhase.fill(0.0f);
 128     mOutputAccum.fill(0.0f);
 129     mFftBuffer.fill(complex_f{});
 130     mAnalysisBuffer.fill(FrequencyBin{});
 131     mSynthesisBuffer.fill(FrequencyBin{});
 132
 133     std::fill(std::begin(mCurrentGains), std::end(mCurrentGains), 0.0f);
 134     std::fill(std::begin(mTargetGains),  std::end(mTargetGains),  0.0f);
 135 }
 136
 137 void PshifterState::update(const ContextBase*, const EffectSlot *slot,
 138     const EffectProps *props, const EffectTarget target)
 139 {
 140     const int tune{props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune};
 141     const float pitch{std::pow(2.0f, static_cast<float>(tune) / 1200.0f)};
 142     mPitchShiftI = clampu(fastf2u(pitch*MixerFracOne), MixerFracHalf, MixerFracOne*2);
 143     mPitchShift  = static_cast<float>(mPitchShiftI) * float{1.0f/MixerFracOne};
 144
 145     static constexpr auto coeffs = CalcDirectionCoeffs({0.0f, 0.0f, -1.0f});
 146
 147     mOutTarget = target.Main->Buffer;
 148     ComputePanGains(target.Main, coeffs.data(), slot->Gain, mTargetGains);
 149 }
 150
 151 void PshifterState::process(const size_t samplesToDo,
 152     const al::span<const FloatBufferLine> samplesIn, const al::span<FloatBufferLine> samplesOut)
 153 {
 154     /* Pitch shifter engine based on the work of Stephan Bernsee.
 155      * http://blogs.zynaptiq.com/bernsee/pitch-shifting-using-the-ft/
 156      */
 157
 158     /* Cycle offset per update expected of each frequency bin (bin 0 is none,
 159      * bin 1 is x1, bin 2 is x2, etc).
 160      */
 161     constexpr float expected_cycles{al::numbers::pi_v<float>*2.0f / OversampleFactor};
 162
 163     for(size_t base{0u};base < samplesToDo;)
 164     {
 165         const size_t todo{minz(StftStep-mCount, samplesToDo-base)};
 166
 167         /* Retrieve the output samples from the FIFO and fill in the new input
 168          * samples.
 169          */
 170         auto fifo_iter = mFIFO.begin()+mPos + mCount;
 171         std::copy_n(fifo_iter, todo, mBufferOut.begin()+base);
 172
 173         std::copy_n(samplesIn[0].begin()+base, todo, fifo_iter);
 174         mCount += todo;
 175         base += todo;
 176
 177         /* Check whether FIFO buffer is filled with new samples. */
 178         if(mCount < StftStep) break;
 179         mCount = 0;
 180         mPos = (mPos+StftStep) & (mFIFO.size()-1);
 181
 182         /* Time-domain signal windowing, store in FftBuffer, and apply a
 183          * forward FFT to get the frequency-domain signal.
 184          */
 185         for(size_t src{mPos}, k{0u};src < StftSize;++src,++k)
 186             mFftBuffer[k] = mFIFO[src] * gWindow.mData[k];
 187         for(size_t src{0u}, k{StftSize-mPos};src < mPos;++src,++k)
 188             mFftBuffer[k] = mFIFO[src] * gWindow.mData[k];
 189         forward_fft(al::as_span(mFftBuffer));
 190
 191         /* Analyze the obtained data. Since the real FFT is symmetric, only
 192          * StftHalfSize+1 samples are needed.
 193          */
 194         for(size_t k{0u};k < StftHalfSize+1;k++)
 195         {
 196             const float magnitude{std::abs(mFftBuffer[k])};
 197             const float phase{std::arg(mFftBuffer[k])};
 198
 199             /* Compute the phase difference from the last update and subtract
 200              * the expected phase difference for this bin.
 201              *
 202              * When oversampling, the expected per-update offset increments by
 203              * 1/OversampleFactor for every frequency bin. So, the offset wraps
 204              * every 'OversampleFactor' bin.
 205              */
 206             const auto bin_offset = static_cast<float>(k % OversampleFactor);
 207             float tmp{(phase - mLastPhase[k]) - bin_offset*expected_cycles};
 208             /* Store the actual phase for the next update. */
 209             mLastPhase[k] = phase;
 210
 211             /* Normalize from pi, and wrap the delta between -1 and +1. */
 212             tmp *= al::numbers::inv_pi_v<float>;
 213             int qpd{float2int(tmp)};
 214             tmp -= static_cast<float>(qpd + (qpd%2));
 215
 216             /* Get deviation from bin frequency (-0.5 to +0.5), and account for
 217              * oversampling.
 218              */
 219             tmp *= 0.5f * OversampleFactor;
 220
 221             /* Compute the k-th partials' frequency bin target and store the
 222              * magnitude and frequency bin in the analysis buffer. We don't
 223              * need the "true frequency" since it's a linear relationship with
 224              * the bin.
 225              */
 226             mAnalysisBuffer[k].Magnitude = magnitude;
 227             mAnalysisBuffer[k].FreqBin = static_cast<float>(k) + tmp;
 228         }
 229
 230         /* Shift the frequency bins according to the pitch adjustment,
 231          * accumulating the magnitudes of overlapping frequency bins.
 232          */
 233         std::fill(mSynthesisBuffer.begin(), mSynthesisBuffer.end(), FrequencyBin{});
 234
 235         constexpr size_t bin_limit{((StftHalfSize+1)<<MixerFracBits) - MixerFracHalf - 1};
 236         const size_t bin_count{minz(StftHalfSize+1, bin_limit/mPitchShiftI + 1)};
 237         for(size_t k{0u};k < bin_count;k++)
 238         {
 239             const size_t j{(k*mPitchShiftI + MixerFracHalf) >> MixerFracBits};
 240
 241             /* If more than two bins end up together, use the target frequency
 242              * bin for the one with the dominant magnitude. There might be a
 243              * better way to handle this, but it's better than last-index-wins.
 244              */
 245             if(mAnalysisBuffer[k].Magnitude > mSynthesisBuffer[j].Magnitude)
 246                 mSynthesisBuffer[j].FreqBin = mAnalysisBuffer[k].FreqBin * mPitchShift;
 247             mSynthesisBuffer[j].Magnitude += mAnalysisBuffer[k].Magnitude;
 248         }
 249
 250         /* Reconstruct the frequency-domain signal from the adjusted frequency
 251          * bins.
 252          */
 253         for(size_t k{0u};k < StftHalfSize+1;k++)
 254         {
 255             /* Calculate the actual delta phase for this bin's target frequency
 256              * bin, and accumulate it to get the actual bin phase.
 257              */
 258             float tmp{mSumPhase[k] + mSynthesisBuffer[k].FreqBin*expected_cycles};
 259
 260             /* Wrap between -pi and +pi for the sum. If mSumPhase is left to
 261              * grow indefinitely, it will lose precision and produce less exact
 262              * phase over time.
 263              */
 264             tmp *= al::numbers::inv_pi_v<float>;
 265             int qpd{float2int(tmp)};
 266             tmp -= static_cast<float>(qpd + (qpd%2));
 267             mSumPhase[k] = tmp * al::numbers::pi_v<float>;
 268
 269             mFftBuffer[k] = std::polar(mSynthesisBuffer[k].Magnitude, mSumPhase[k]);
 270         }
 271         for(size_t k{StftHalfSize+1};k < StftSize;++k)
 272             mFftBuffer[k] = std::conj(mFftBuffer[StftSize-k]);
 273
 274         /* Apply an inverse FFT to get the time-domain signal, and accumulate
 275          * for the output with windowing.
 276          */
 277         inverse_fft(al::as_span(mFftBuffer));
 278
 279         static constexpr float scale{3.0f / OversampleFactor / StftSize};
 280         for(size_t dst{mPos}, k{0u};dst < StftSize;++dst,++k)
 281             mOutputAccum[dst] += gWindow.mData[k]*mFftBuffer[k].real() * scale;
 282         for(size_t dst{0u}, k{StftSize-mPos};dst < mPos;++dst,++k)
 283             mOutputAccum[dst] += gWindow.mData[k]*mFftBuffer[k].real() * scale;
 284
 285         /* Copy out the accumulated result, then clear for the next iteration. */
 286         std::copy_n(mOutputAccum.begin() + mPos, StftStep, mFIFO.begin() + mPos);
 287         std::fill_n(mOutputAccum.begin() + mPos, StftStep, 0.0f);
 288     }
 289
 290     /* Now, mix the processed sound data to the output. */
 291     MixSamples({mBufferOut.data(), samplesToDo}, samplesOut, mCurrentGains, mTargetGains,
 292         maxz(samplesToDo, 512), 0);
 293 }
 294
 295
 296 struct PshifterStateFactory final : public EffectStateFactory {
 297     al::intrusive_ptr<EffectState> create() override
 298     { return al::intrusive_ptr<EffectState>{new PshifterState{}}; }
 299 };
 300
 301 } // namespace
 302
 303 EffectStateFactory *PshifterStateFactory_getFactory()
 304 {
 305     static PshifterStateFactory PshifterFactory{};
 306     return &PshifterFactory;
 307 }